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INTRODUCTION 


Research  under  this  contract  began  as  scheduled  on  October  1,  1981. 
During  the  first  twelve  months,  no  significant  deviations  from  the  plan 
contained  in  our  proposal  dated  May  13,  1981,  occurred.  Research  performed 
relative  to  each  of  the  tasks  listed  in  our  Statement  of  Work,  dated 
September  21,  1981,  is  summarized  in  the  following  paragraphs.  Further 
details  regarding  some  of  these  topics  are  provided  in  the  attached  appen¬ 
dices,  in  our  Semi-Annual  Report  dated  August,  1982,  and  in  videotapes 
previously  furnished  to  DARPA. 

RESEARCH  ACCOMPLISHED 
Task  1:  Vertical  Sensor  Evaluation 

An  aircraft-quality  vertical  gyro  has  been  purchased  and  evaluated 
using  the  OSU  Hexapod  as  a  test  bed.  Test  results  are  documented  in  the 
attached  Appendix  3.  This  gyro  has  been  judged  to  be  fully  satisfactory 
and  is  a  candidate  for  the  ASV-84  vertical  sensor. 

Task  2:  Control  Mode  Studies 

As  a  result  of  research  coordination  meetings  held  during  the  second 
half  of  this  contract  year,  a  total  of  six  control  modes  have  been  defined 
.jr  the  ASV-84  vehicle.  These  modes  were  formalized  in  an  internal  memorandum 
entitled  "Operational  Modes  and  Computer  Architecture  for  ASV-84  Vehicle," 
dated  July,  1982.  Copies  of  this  memorandum  were  distributed  to  DARPA  and 
all  ASV-84  contractors  during  a  meeting  at  the  University  of  Wisconsin  in 
August.  Appendix  2  provides  a  summary  of  the  projected  characteristics  of 
each  control  mode.  It  was  agreed  during  the  August  meeting  that  highest 
priority  for  software  development  would  be  attached  to  the  terrain-following 
mode . 


Task  3:  Insect  Control  Studies 


Subcontracted  work  at  the  University  of  Alberta  to  analyze  climbing 
and  ditch-crossing  strategies  used  by  locusts  has  been  completed.  A  copy 
of  the  subcontractor's  report  and  a  videotape  illustrating  major  findings 
of  this  research  have  been  provided  to  DARPA.  One  of  the  most  important 
outcomes  of  this  work  was  the  discovery  that  locusts  abandon  wave  gaits 
for  rough-terrain  locomotion  and  use  instead  a  class  of  gaits  more  suited 
to  traversing  large  obstacles.  These  gaits  will  be  investigated  for 
application  to  the  OSU  Hexapod  and  the  ASV-84  during  the  coming  contract 
year. 

Task  4:  Fault  Tolerant  Software 

A  first  version  of  the  safety  software  for  the  ASV-84  vehicle  has 
been  tested  on  the  OSU  Hexapod.  This  software  correctly  detects  imminent 
collisions  of  joints  with  their  limits,  of  limbs  with  other  limbs,  and 
of  the  body  with  the  ground  (resulting  from  static  instability).  When 
such  conditions  occur,  the  Hexapod  is  stopped  automatically  and  an  error 
message  is  pr'  '^ed  to  the  operator.  Details  of  this  work  are  provided 
in  an  M.S.  thesis  previously  furnished  to  DARPA,  dated  August,  1982,  and 
entitled  "Safety  Checking  System  with  Voice  Response  for  the  OSU  Hexapod." 

Task  5:  Laser  Foothold  Designator 

Hardware  and  software  for  triangulation  by  the  OSU  Hexapod  on 
optically-designated  footholds  has  been  completed.  The  accuracy  of  this 
system  exceeds  that  of  the  leg  servos.  Closed  loop  testing  using  follow- 
the-leader  gaits  with  a  hand-held  Helium-Neon  laser  foothold  designator 
is  scheduled  for  the  coming  q  tarter  of  this  research  program. 


Task  6:  Leg  Geometry  Design 


There  have  been  a  number  of  important  developments  in  this  area 
during  this  contract  year.  The  DUWE  vehicle  (Dynamic  Unpowered  Walking 
Experiment)  was  successfully  operated  on  a  tilted  plane  with  a  slope  of 
5  percent,  corresponding  to  a  specific  resistance  of  only  .05.  This 
demonstrates  that  there  is  no  intrinsic  mechanical  obstacle  to  operation 
of  suitably  designed  walking  machines  at  power  levels  competitive  with 
automotive  vehicles.  Agreement  with  computer  simulation  results  was 
also  satisfactory.  A  complete  discussion  of  this  experiment  is  given  in 
the  thesis  "Dynamic  Study  of  a  Four-Bar  Linkage  Walking  Machine  Leg"  by 
T.  Frank  Brown,  Jr.,  a  copy  of  which  has  been  supplied  to  DARPA.  A  video¬ 
tape  of  the  DUWE  in  operation  has  also  been  provided. 

Dr.  Shigeo  Hirose  was  appointed  as  Visiting  Associate  Professor  in 
the  Department  of  Mechanical  Engineering  from  April  1st  to  May  31st  and 
was  supported  by  this  project.  He  attended  the  contractors’  meeting  in 
May  and  assisted  with  all  aspects  of  the  project  but,  particularly,  leg 
geometry  design.  Dr.  Hirose  was  unable  to  accept  the  budgeted  subcontract 
because  of  differences  in  the  organization  and  philosophy  of  research 
sponsorship  in  Japan.  In  lieu  of  that,  it  was  agreed  that  he  should  be 
invited  to  make  a  similar  extended  visit  next  year. 

Strength  and  deflection  studies  of  the  candidate  leg  designs  have 
progressed  substantially  and  a  detailed  structural  design  of  a  breadboard 
leg  has  been  completed.  It  was  found  that  the  leg  configuration  with  a 
four-bar  mechanism  and  sliding  shank  could  not  be  satisfactory  embodied 
in  hardware.  This  configuration  has,  therefore,  been  abandoned.  The 
first  configuration  to  be  tested  in  the  oreadboard  leg  rig  will  be  a  panto¬ 
graph.  All  structural  members  for  this  leg  have  been  completed.  It  is 


anticipated  that  final  assembly  and  initial  testing  will  take  place  early 
in  the  next  quarter  of  this  project. 

A  fully  serviced  laboratory  space  has  been  commissioned  for  ASV 
hydraulic  system  testing.  It  is  a  ground-floor  level  bay  with  440  square 
feet  of  area  and  a  large  roller  door  to  the  outside  of  the  building.  It 
is  equipped  with  an  overhead  hoist.  This  space  will  be  used  initially 
for  the  breadboard  leg  tests  and  subsequently  for  the  assembly  of  ASV 
subsystems. 

Task  7:  Power  Package  Specifications 

Discussions  continued  with  the  University  of  Wisconsin  on  the 
specifications  for  this  unit.  Motor,  flywheel,  and  alternator  capacities 
have  now  been  settled.  Final  pump  specifications  await  completion  of 
operational  mode  definitions  for  the  hydraulic  circuits.  The  overall 
configuration  of  an  interim  system  for  use  in  preliminary  tests  has  been 
decided  on.  A  lightweight,  high  performance  motorcycle  engine  will  be 
used. 

Task  8:  Power  Transmission  and  Actuator  Servo  System  Design 

Simulation  studies  of  proposed  circuit  configurations  are  continuing. 
Modifications  of  the  drive  and  lift  circuit  configurations  to  deal  with 
problems  of  operation  in  the  close  maneuvering  mode  and  of  supplying  leg 
return  energy  are  being  studied. 

The  major  hydraulic  components  of  the  breadboard  leg  system  have 
been  delivered  and  tested.  They  have  been  found  satisfactory  and  will 
be  mounted  ori  the  breadboard  leg  for  functional  testing  during  the  coming 
quarter. 
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The  design  of  the  lift  circuit  for  the  breadboard  leg  is  being 
done  by  Battelle  in  coordination  with  OSU.  A  regenerative  circuit  arrange¬ 
ment  to  give  rapid  leg  extension  if  footing  is  lost  is  being  studied. 

While  on  a  visit  to  Australia  for  other  purposes  in  July,  Dr. 

Waldron  also  visited  the  University  of  Western  Australia  at  which  a 
hydraulically  actuated  robot  sheep-shearing  system  is  being  designed 
and  tested.  Observations  of  that  system  were  also  relevant  to  design 
of  the  ASV  system.  Dr.  Waldron  also  visited  Ifield  Corporation,  Dural, 

New  South  Wales,  Australia,  a  manufacturer  of  very  high  efficiency  servo- 
hydraulic  pumps.  Verified  performance  figures  are  superior  to  those  of 
U.S.  suppliers  presently  under  consideration.  Delivery  time  and  costs 
would  probably  preclude  use  of  Ifield  pumps  for  the  1984  ASV,  but  the 
possibility  is  being  further  explored. 

An  Intellec  Model  286  Series  III  Microcomputer  Development  System 
has  been  donated  to  the  project  by  Intel  for  use  in  the  Mechanical  Engi¬ 
neering  Department  for  actuator  servo  design  and  development  of  the 
breadboard  leg  controller.  The  breadboard  leg  computer  has  been  delivered 
and  control  programs  are  currently  under  development. 

Task  9:  Overall  Structural  Design 

Field  tests  to  study  mobility  in  forest  and  steep  terrain  conditions 
have  been  caried  out  using  a  light  weight  mock-up  frame.  The  testing  was 
performed  on  private  property  near  Stockdale  in  Southern  Ohio.  A  videotape 
showing  the  results  of  these  tests  has  been  supplied  to  DARPA. 

Finite  element  studies  of  strength  and  vibrational  modes  of  the 
vehicle  frame  were  concluded  using  selected  structural  cross-sections. 
Earlier  work  of  this  type  is  shown  on  the  videotape  accompanying  the  final 


report  on  contract  number  MDA903-81-C-0138.  Detailed  manufacturing 
drawings  of  the  frame  have  been  completed.  A  copy  of  these  drawings 
has  been  forwarded  to  the  University  of  Wisconsin  to  permit  construction 
of  a  cockpit  mockup  for  control  studies.  In  addition,  a  decision  was 
made  to  construct  a  full-scale  wooden  mockup  of  the  entire  frame  at  OSU 
to  support  studies  of  internal  system  packaging,  wiring  layout,  etc. 
Construction  of  this  frame  will  be  completed  early  in  the  coming  quarter. 

Task  10:  On-Board  Computer  Design 

The  Intel  8086  with  the  8087  co-processor  has  been  selected  as  the 
most  suitable  microprocessor  for  the  on-board  computer.  These  two  chips 
are  available  along  with  appropriate  analog-to-digital  and  digital-to- 
analog  converters  on  the  Intel  8630  single-board  computer.  A  first-cut 
multiprocessor  computer  architectural  design  has  been  completed  and  docu¬ 
mented  in  the  internal  memorandum  referenced  under  Task  2.  An  Intellec 
Series  III  Microprocessor  Development  System  suitable  for  programming  this 
computer  has  been  delivered.  Construction  of  a  breadboard  version  of  this 
computer  is  scheduled  to  begin  during  the  coming  quarter. 

Task  11:  Software  Design 

Software  for  terrain-following  locomotion  by  the  OSU  Hexapod  has 
been  completed  and  tested.  Details  are  presented  in  the  attached  Appendix 
3.  The  greater  part  of  this  software  is  equally  suited  to  the  ASV-84 
vehicle.  Conversion  to  ASV-84  control  and  transfer  of  the  software  to 
the  breadboard  multiprocessor  computer  will  begin  next  quarter. 

Task  12:  Electronic  Subsystem  Design 

Load  cells  for  the.  ASV-84  foot-force  sensors  have  been  designed. 
Transducers  and  amplifiers  have  been  selected.  Component  orders  have  been 
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placed  for  one  cell  to  be  tested  on  the  ASV-84  breadboard  leg.  A  servo¬ 
valve  controller  for  the  breadboard  leg  has  been  completed  and  tested. 

In  addition  to  this  work,  the  construction  and  bench  testing  of  an  ultra¬ 
sonic  ranging  system  has  been  completed.  This  system  will  be  replicated 
and  tested  as  a  foot  proximity  sensor  on  the  OSU  Hexapod  during  the  next 
contract  year.  Optical  alternatives  to  ultrasonic  proximity  sensing  will 
also  be  studied  with  the  assistance  of  CEN,  Saclay,  France,  under  an 
existing  subcontract. 
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Chapter  1 
INTRODUCTION 


1.1  Background 

Throughout  history,  man  has  continually  developed  better 
transportation  systems.  In  the  beginning  he  had  to  rely  on  his  own 
two  legs  for  transportation.  As  he  became  the  master  of  his  erviron- 
ment,  he  domesticated  animals  and  invented  the  boat  and  wheel.  Today, 
man  can  fly  aircraft  faster  than  sound,  take  a  submarine  under  polar 
icecaps,  and  fly  spacecraft  to  the  moon.  For  personal  transportation, 
the  automobile  has  given  him  mobility  never  before  possible.  Trans¬ 
portation  over  very  rough  terran,  however,  is  one  area  in  which  little 
progress  has  been  made. 

Various  schemes  for  locomotion  over  highly  irregular  surfaces 
have  been  tried.  Perhaps  the  most  common  off-road  machines  are  multi¬ 
wheel  drive  vehicles  and  tracked  vehicles.  These  are  quite  successful 
for  moderately  rough  terrain.  However,  even  the  most  versatile  of 
these  machines  is  vastly  inferior  to  natural  legged  systems  (animals) 
on  very  difficult  surfaces  [1]. 

The  primary  difficulty  with  designing  highly  terrain-adaptive 
vehicles  lies  in  the  complexity  of  the  control  task.  While  a  vehicle 
such  as  an  automobile  possesses  only  two  controlled  degrees  of  freedom, 


the  simplest  model  of  a  natural  quadraped  requires  twelve  degrees  of 
freedom.  It  has  been  shown  that  controlling  twelve  degrees  of  freedom 
is  too  difficult  a  task  for  a  human  [2].  Until  recently,  this  repre¬ 
sented  an  insurmountable  obstacle  to  the  development  of  highly  adaptive 
vehicles.  Now,  however,  the  advent  of  miniature,  low-cost  digital 
computers  has  made  it  possible  to  relieve  the  operator  of  the  coordi¬ 
nation  task. 

In  the  early  70's,  researchers  at  The  Ohio  State  University  began 
studying  the  problem  of  computer  control  of  a  multi-jointed  vehicle  [3]. 
To  aid  in  this  study,  the  OSU  Hexapod  Vehicle  was  constructed  as  a 
laboratory  testbed.  This  legged  vehicle  possesses  six  legs  with  three 
degrees  of  freedom  per  leg,  for  a  total  of  eighteen  degrees  of  freedom. 
Prior  to  the  work  on  this  thesis,  solutions  to  the  fundamental  control 
problems  had  been  found,  including  leg  gait  specification,  foot  posi¬ 
tion  specification  to  achieve  the  desired  gait,  and  joint  coordination 
to  achieve  the  desired  foot  position.  The  Hexapod  had  the  ability  to 
walk  forward,  backward,  sideways,  turn,  or  execute  a  combination  of 
these  maneuvers  simultaneously.  However,  it  could  walk  only  on  level 
surfaces,  due  to  the  fact  that  it  lacked  the  sensors  to  detect  surface 
irregularities . 

The  problem  of  locomotion  over  irregular  terrain  had  been 
studied  in  simulation,  and  one  leg  had  been  equipped  with  vector  force 
sensors  [4],  A  method  of  obstacle  accommodation  had  been  developed 
for  that  only  leg,  enabling  obstacles  to  be  placed  under  the  leg  with¬ 
out  impeding  vehicle  motion.  However,  the  level  of  sophistication  of 
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the  control  software  and  the  amount  of  sensor  hardware  was  still 
inadequate  to  allow  locomotion  over  random  terrain.  The  objective  of 
the  work  presented  in  this  thesis  is  to  fully  equip  the  OSU  Hexapod 
Vehicle  with  the  necessary  sensors  and  to  develop  the  necessary  control 
software  to  achieve  locomotion  over  random  terrain  with  only  direction¬ 
al  commands  being  provided  by  the  human  operator. 

1 .2  Organization 

A  description  of  the  existing  systems  of  the  OSU  Hexapod  Vehicle 
is  given  in  Chapter  2,  including  the  mechanical  hardware,  electronics, 
and  control  software.  Chapter  3  details  the  improvements  made  to  the 
vehicle  system  in  the  areas  of  electronics,  force  sensors,  attitude 
sensors,  and  utility  software.  The  control  algorithms  necessary  for 
adaptive  locomotion  are  developed  in  Chapter  4.  Results  of  locomotion 
tests  are  given  in  Chapter  5.  Chapter  6  summarizes  the  contributions 
made  in  this  work  and  suggests  areas  where  further  work  is  needed. 
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Chapter  2 


REVIEW  OF  EXISTING  SYSTEMS 
OF  THE  OSU  HEXAPOD  VEHICLE 


2.1  Introduction 

The  OSU  Hexapod  Vehicle  is  a  complex  system  consisting  of 
mechanical  hardware,  electronic  hardware,  and  software  control  algor¬ 
ithms.  In  this  chapter,  all  aspects  of  the  vehicle  which  relate  to 
autopilot  design  will  be  discussed.  The  systems  are  described  as  they 
existed  prior  to  the  modifications  and  additions  which  are  detailed  in 
subsequent  chapters.  Complete  details  of  the  vehicle  design  can  be 
found  in  the  referenced  literature. 

2.2  Mechanical  Hardware 

A  photograph  of  the  OSU  Hexapod  Vehicle  is  shown  in  Figure  2.1. 
The  vehicle  structure  consists  of  six  legs  mounted  on  an  aluminum 
frame.  Each  leg  possesses  three  independently  powered  joints  arranged 
in  an  arthropod  configuration.  Each  joint  actuator  consists  of  an 
industrial  grade  series-wound  electric  drill  motor  and  a  gear  reduction 
unit.  Figure  2.2  is  a  diagram  of  the  actuator  system.  The  second 
stage  of  the  gear  reduction  is  a  non-backdri veabl e  worm  gear.  This 
insures  that  the  joints  lock  into  position  when  power  is  removed,  but 
makes  the  relationship  between  motor  shaft  torque  and  applied  foot 
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Figure  2.1.  The  OSU  Hexapod  Vehicle. 


force  highly  nonlinear.  Complete  specifications  on  the  joint  actuator 
system  can  be  found  in  [5]. 

2.3  Instrumentation 

Each  joint  is  instrumented  with  a  potentiometer  and  a  tachometer 
to  measure  position  and  rate,  respectively.  The  tachometer  is  mounted 
directly  to  the  motor  output  shaft,  while  the  potentiometer  is  mounted 
at  the  output  of  the  gear  reduction  unit.  The  tachometer  location  in¬ 
sures  that  the  rate  information  is  not  affected  by  gear  backlash.  The 
position  feedback  loop  is  affected  by  gear  backlash,  however,  so  care 
must  be  taken  to  avoid  limit  cycling  when  designing  the  servo  loop. 

In  1977,  one  leg  of  the  vehicle  was  equipped  with  vector  force 
sensors.  The  lateral  forces  are  measured  by  semiconductor  strain 
gauges  mounted  to  the  sides  of  the  lower  leg  segment.  The  axial  force 
component  is  measured  by  a  semiconductor  load  cell  mounted  in  the  foot 
Amplification  circuitry  is  located  inside  the  leg  segment.  The  force 
sensor  design  is  illustrated  in  Figure  2.3,  and  is  described  in  detail 
in  [6]. 

2.4  Electronics 

2.4.1  Motor  Controller  Circuitry 

The  motors  are  operated  using  half-wave  AC  phr.se  control.  The 
power  control  circuitry  consists  of  a  bridge  rectifier  and  a  triac  for 
each  motor.  Each  triac  is  controlled  by  an  analog  trigger  generator 
circuit  which  compares  the  input  voltage  signal  with  a  reference  wave¬ 
form  and  sends  a  turn-on  pulse  to  the  triac  gate  when  the  line  voltage 
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is  at  the  proper  phase.  The  reference  waveform  is  generated  by  a 
separate  circuit,  and  removes  the  nonlinearity  associated  with  AC  phase 
control.  Complete  schematics  for  the  motor  controller  circuitry  can  be 
found  in  [5].  The  power  circuitry  is  located  on  panels  mounted  be¬ 
tween  leg  pairs;  the  trigger  and  reference  circuitry  is  located  in  the 
card  cage  at  the  rear  of  the  vehicle. 

2.4.2  Instrumentation  Electronics 

The  sensors  installed  on  the  Hexapod  all  require  signal  conditioning 
before  undergoing  analog-to-digital  conversion.  The  potentiometer  out¬ 
puts  are  scaled  by  operational  amplifiers  to  give  a  ten  volt  signal  at 
full  mechanical  deflection.  The  tachometer  outputs  are  scaled  and 
low-pass  filtered  to  eliminate  the  commutator  noise.  The  potentiometer 
and  tachometer  signal  conditioning  circuitry  is  detailed  in  [5],  This 
circuitry  is  located  in  the  card  cage. 

The  first-stage  amplification  circuitry  for  the  force  sensors  is 
located  inside  the  sensor-equipped  leg.  A  second-stage  amplifier  is 
located  in  the  card  cage,  where  the  signal  is  filtered  and  the  gain 
may  be  adjusted.  Force  amplifier  schematics  can  be  found  in  [6]. 

2.4.3  Computer  Interface 

To  expedite  communication  between  the  Hexapod  and  its  control 
computer,  a  special  purpose  digital  data  link  has  been  constructed. 

The  data  link  is  composed  of  two  primary  segments:  the  vehicle  inter¬ 
face  and  the  computer  interface.  There  is  no  direct  electrical  connec¬ 
tion  between  the  two  interfaces;  all  lines  are  optically  isolated  to 


protect  the  computer  in  the  event  of  an  electrical  malfunction  of  the 
Hexapod.  The  data  link  operation  is  outlined  in  the  following  para¬ 
graphs,  and  complete  specifications  can  be  found  in  [7]. 

The  data  path  from  the  computer  to  the  Hexapod  is  defined  as  the 
feedforward  path,  while  the  feedback  path  is  directed  from  the  Hexapod 
to  the  computer.  Only  the  actuator  input  voltages  need  to  be  trans¬ 
mitted  over  the  feedforward  path.  Thus,  there  are  eighteen  words  of 
data  to  be  transmitted  each  time  the  servo  loop  is  executed  by  the 
computer.  Each  word  consists  of  eight  bits,  and  is  transmitted  in 
parallel  along  with  five  bits  of  address  information.  The  data  link 
addresses  are  memory  mapped  into  the  PDP-11 /70  control  computer,  and 
transmission  is  initiated  when  the  computer  performs  a  write  to  one  of 
the  data  link  addresses.  After  allowing  adequate  set-up  time  for  the 
optical  isolators,  the  data  is  strobed  into  one  of  eighteen  registers 
in  the  vehicle  interface,  as  selected  by  the  address  information.  This 
process  requires  a  total  of  40  microseconds,  after  which  time  the  com¬ 
puter  may  transmit  another  word.  A  digi tal -to-analog  converter  associ¬ 
ated  with  each  of  the  data  registers  in  the  vehicle  interface  converts 
the  data  to  an  analog  voltage,  which  is  then  used  as  the  input  voltage 
to  a  triac  trigger  generator.  A  block  diagram  of  the  feedforward 
circuitry  is  shown  in  Figure  2.4. 

The  feedback  portion  of  the  data  link  originally  handled  54 
channels  of  information,  including  18  channels  each  of  position  and 
rate  information.  In  addition,  provision  was  made  for  18  channel  of 
force  information,  although  only  three  were  used  in  the  initial  system 
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Figure  2.4  Functional  Block  Diagram  of  the  Forward  Portion  of 
the  Digital  Data  Link  [7]. 


configuration.  To  implement  the  feedback  without  causing  the  computer  to 
wait  for  conversion,  a  shared  memory  approach  has  been  employed.  A  64  word 
shared  memory,  located  in  the  computer  interface,  is  continually  updated  by 
the  vehicle  interface.  Thus,  the  computer  can  obtain  current  information 
by  simply  performing  a  memory  read  operation.  The  feedback  portion  of  the 
vehicle  interface  contains  an  internal  clock  and  a  modulus  54  counter  to 
generate  addresses.  The  address  information  selects  one  channel  of  a  54 
channel  analog  multiplexer.  The  multiplexer  selects  one  of  the  analog  feed¬ 
back  signal s  ,  which  is  then  input  to  an  anal og-to-digi tal  converter.  The 
resulting  10  bit  digital  word  is  then  transmitted  to  the  computer  interface 
along  with  the  6  address  bits.  An  arbitration  and  control  circuit  in  the  com¬ 
puter  interface  then  resol ves  any  memory  access  confl icts  and  directs  the 
writing  of  the  information  into  the  shared  memory.  One  data  transmission 
requires  1  00  microseconds ;  thus  each  location  in  the  shared  memory  i s  updated 
every  5.4  milliseconds.  A  Block  diagram  of  the  feedback  circuitry  is 
shown  in  Figure  2.5. 

2.5  Software 

2.5.1  Software  Organization 

The  control  software  for  the  OSU  Hexapod  Vehicle  is  arguably  the 
most  complex  part  of  the  vehicle  system.  The  control  algorithm  design 
is  simplified  by  partitioning  the  task  into  well-defined  functional 
blocks,  or  subtasks,  with  a  minimal  amount  of  communication  between 
the  subtasks.  Although  the  current  control  program  (Hexapod  Control 
Program  version  3.0)  is  the  result  of  years  of  work  by  many  researcners, 
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Figure  2.5  Functional  Block  Diagram  of  the  Feedback  Portion 
of  the  Digital  Data  Link  [7], 


the  partitioning  problem  is  still  not  perfectly  understood.  Version 
3.0  software  represents  a  continuing  effort  to  define  a  structured 
algorithm  which  can  be  reflected  in  the  software  architecture . 

The  control  task  partitioning  of  Hexapod  Control  Program  version 
3.0  is  illustrated  in  Figure  2.6.  The  figure  shows  the  general  direc¬ 
tion  of  information  flow  among  the  subtasks.  The  particular  algorithm 
implemented  in  a  given  functional  block  can  vary,  depending  on  the  type 
of  motion  to  be  performed  by  the  vehicle.  The  parameters  which  are 
passed  between  tne  functional  blocks  are  not,  in  general,  precisely 
specified,  but  rather  vary  somewhat  depending  on  which  algorithms  are 
being  executed. 

Figure  2.7  shows  the  major  routines  of  version  3.0  and  their 
relationship  to  the  control  structure  of  Figure  2.6.  Each  block  in 
Figure  2.7  is  implemented  as  a  subroutine,  with  the  exception  of  the 
executive  software.  The  software  is  largely  self-documenting;  the 
parameters  required  by  a  given  subroutine  are  identified  in  the  sub¬ 
routine  header. 

2.5.2  Executive  Software 

The  executive  software  provides  the  interface  between  the  human 
operator  and  the  vehicle  system  via  a  CRT  terminal.  The  operator 
enters  commands  through  the  keyboard,  specifying  parameters  such  as 
vehicle  speed,  direction,  operating  mode,  etc.  The  current  vehicle 
status  is  displayed  on  the  CRT,  along  with  a  list  of  valid  operator 
commands.  Program  flow  is  established  by  the  executive  software  as  a 
function  of  the  operator  inputs. 
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Figure  2.6  Control  Task  Partitioning  of  Hexapod  Control  Program  Version  3.0. 
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Figure  2.7  Software  Organization  of  Hexapod  Control 
Program  Version  3.0. 


2.5.3  Body  Motion  Planning 

The  body  motion  planning  functional  block  is  responsible  for 
conditioning  operator  speed  and  direction  commands  so  that  they  can  be 
implemented  by  the  lower  level  routines.  This  includes  low-pass  fil¬ 
tering  the  input  velocities  and  limiting  the  velocities  to  realizable 
values.  For  some  simple  motion  classes*  this  functional  block  can  be 
el iminated. 

2.5.4  Leg  Coordination 

The  leg  coordination  algorithms  are  designed  to  insure  that  the 
vehicle  is  always  supported  by  three  or  more  legs.  In  addition,  the 
legs  must  not  collide  with  other  legs.  For  ambulatory  motion,  these 
conditions  are  satisfied  implicitly  by  implementing  periodic  gaits  [8] 
Other  algorithms,  such  as  the  normalization  routine,  satisfy  these 
conditions  explicitly. 

2.5.5  Foot  Trajectory  Planning 

Hexapod  Control  Program  version  3.0  contains  two  foot  trajectory 
planning  routines,  FOOTPATH  and  FOOTLINE.  The  most  important  of  these, 
FOOTPATH,  is  used  during  wave  gait  implementation.  In  this  routine, 
the  trajectory  of  a  given  foot  is  divided  into  two  phases:  support 
phase  and  transfer  phase.  A  foot  is  in  support  phase  when  it  is  on  the 
ground  supporting  a  portion  of  the  vehicle  weight.  Vehicle  direction 
and  speed  are  determined  by  the  trajectories  of  the  feet  in  support 
phase.  Algorithms  have  been  implemented  in  version  3.0  software  which 
allow  the  vehicle  to  exhibit  three  ambulatory  modes  [9,10],  In  cruise 


mode,  the  vehicle  may  walk  forward  or  backward,  turn  with  a  minimum 
radius  of  60  inches,  crab  at  up  to  a  45  degree  angle  from  the  vehicle 
heading,  or  exhibit  all  of  these  motions  simultaneously.  Turn-in-place 
mode  allows  the  vehicle  to  perform  a  pure  rotational  motion  about  its 
geometrical  center,  and  sidestep  mode  implements  pure  sideways  loco¬ 
motion.  With  the  system  configured  as  it  was  prior  to  the  work  of  this 
thesis,  locomotion  in  any  of  these  modes  is  possible  only  over  rela¬ 
tively  smooth,  level  ground. 

When  a  leg  reaches  the  end  of  its  support  phase,  it  enters 
transfer  phase.  In  transfer  phase,  the  foot  lifts  off  of  the  ground 
and  moves  to  the  point,  predicted  by  an  algorithm  in  [10],  where  it  will 
next  enter  support  phase.  The  foot  follows  a  half-sine  wave  trajectory 
through  the  air  in  transfer  phase.  Switching  from  transfer  phase  to 
support  phase  is  done  as  a  function  of  the  kinematic  cycle  phase  vari¬ 
able. 

The  second  foot  trajectory  planning  routine,  FOOTLINE,  moves  all 
six  feet  along  arbitrary  and  independent  straight-line  trajectories. 

This  is  useful  for  simple  procedures  such  as  the  initializing  of  leg 
positions.  In  version  3.0,  FOOTLINE  is  used  to  implement  all  motions 
except  ambulatory  motion.  The  output  of  both  foot  trajectory  routines 
is  the  desired  position  and  desired  rate  of  a  foot,  expressed  in  body 
coordinates . 
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2.5.6  Jacobian  Servo  Control 

The  function  of  the  Jacobian  servo  routine  is  to  cause  the 
vehicle  legs  to  follow  the  trajectories  specified  by  the  foot  trajectory 
planning  routines.  The  Jacobian  servo  routine  implemented  in  version 
3.0  has  two  servo  levels.  The  inner  loop  is  a  rate  feedback  loop  which 
controls  the  angular  rate  of  a  given  actuator.  The  outer  loop  is  closed 
in  rectangular  body  coordinates  and  controls  the  position  and  velocity 
of  a  foot  tip.  A  block  diagram  of  the  control  structure  is  shown  in 
Figure  2.8,  and  definitions  of  the  control  parameters  are  given  in 
Table  2.1  . 

The  Jacobian  servo  derives  its  name  from  the  use  of  the  inverse 
Jacobian  matrix  to  transform  between  rectilinear  rates  and  joint  rates, 
where  the  Jacobian  matrix  is  the  matrix  of  partial  derivatives  of 
rectilinear  foot  position  coordinates  as  functions  of  joint  angles. 

The  general  Jacobian  control  problem  is  discussed  in  [11],  and  the 
specific  implementation  used  for  control  of  the  OSU  Hexapod  Vehicle  is 
well  documented  in  [12]. 
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TABLE  2.1 

JACOBIAN  CONTROL  PARAMETER  DEFINITIONS 
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(desired  foot  position  expressed 
in  body  coordinates) 

(actual  foot  position  expressed 
in  body  coordinates) 

(desired  foot  velocity  expressed 
in  body  coordinates) 

(actual  foot  velocity  expressed 
in  body  coordinates) 

(commanded  foot  velocity  ex¬ 
pressed  in  body  coordinates) 

(vector  of  joint  rate  commands) 

(vector  of  joint  rate  errors) 

(vector  of  joint  actuator  input 
vol tages) 

(vector  of  actual  joint  angles) 
(vector  of  actual  joint  rates) 


(position  gain  matrix) 


J(-3)  =  Jacobian  matrix  which  converts  from  joint  rates 
to  rectilinear  foot  velocity 

Kinematics  =  equations  which  convert  joint  angles  to 
rectilinear  foot  coordinates 
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Chapter  3 

HEXAPOD  SYSTEM  MODIFICATIONS  AND  ADDITIONS 


3.1  Introduction 

The  OSU  Hexapod  Vehicle,  as  described  in  Chapter  2,  did  not  have 
the  capability  of  walking  on  uneven  terrain.  The  vehicle  configura¬ 
tion  was  designed  for  the  purpose  of  rough-terrain  locomotion,  but  was 
unable  to  utilize  its  highly  flexible  geometry  for  this  purpose  due  to 
a  lack  of  sensor  hardware  and  control  software.  The  proposed  vehicle 
autopilot  must  allow  the  Hexapod  to  traverse  irregular  terrain  while 
maintaining  the  vehicle  pitch  and  roll  attitudes  within  acceptable 
limits  with  only  directional  inputs  being  provided  by  a  human  operator. 

To  implement  the  autopilot  function,  the  Hexapod  system  required 
new  software  control  algorithms  and  additional  sensing  hardware.  The 
addition  of  vector  force  sensors  or.  all  legs  of  the  Hexapod  provides 
ground  reaction  force  information,  and  a  vehicle  attitude  sensor  pro¬ 
vides  the  needed  information  about  body  pitch  and  roll.  The  sensor 
hardware  design  is  detailed  in  this  chapter,  along  with  system  hard¬ 
ware  modifications  necessary  to  accommodate  the  sensors.  Additions 
made  in  the  interest  of  improving  system  reliability  are  also  docu¬ 
mented  in  this  chapter.  A  discussion  of  the  autopilot  control  algor¬ 
ithms  is  presented  in  Chapter  4. 
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3.2  Force  Sensors 

As  noted  in  section  2.3,  one  leg  of  the  Hexapod  was  equipped 
with  vector  force  sensors  in  1977.  This  sensor  design,  which  is 
described  in  section  2.3  and  detailed  in  [6],  was  evaluated  as  a 
basis  for  the  new  sensors.  The  design  had  proven  to  be  reliable  over 
the  three  years  it  was  in  operation,  and  the  sensor  performance  was 
demonstrated  to  be  adequate  in  [6  ]  and  [7  ].  As  a  result,  the  lateral 
force  sensor  design  was  retained,  and  consists  of  two  semiconductor 
strain  gauges  mounted  on  adjacent  faces  of  the  lower  limb  segment  of 
the  Hexapod  leg. 

The  amplifier  circuit  configuration  used  with  the  semiconductor 
strain  gauges  is  identical  to  that  used  in  [6J.  It  consists  of  a 
Wheatstone  bridge  composed  of  a  strain  guage  plus  two  external  resis¬ 
tors  and  a  trim  potentiometer.  The  bridge  circuit  is  followed  by  a 
differential  amplifier  which  is  located  inside  the  vehicle  leg.  A 
second  amplification  stage  which  provides  manual  gain  adjustment  and 
low-pass  filtering  is  located  in  the  card  cage  at  the  rear  of  the 
vehicle.  Schematics  for  the  strain  gauge  amplification  circuitry  are 
shown  in  Figure  3.1. 

The  axial  force  sensor  in  the  previous  design  consisted  of  a 
semiconductor  load  cell  mounted  in  an  aluminum  housing,  with  the 
ground  reaction  force  being  transmitted  to  the  load  cell  via  a  steel 
piston.  The  load  cell  used  in  [6]  was  no  longer  available  at  the  time 
of  the  redesign,  and  thus  a  search  of  comparable  commercial  sensors  was 
undertaken.  Three  types  of  load  cells  were  investigated :  semiconductor. 
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strain  gauge,  and  piezoelectric.  The  characteristics  of  these  devices 
are  listed  in  Table  3.1 . 


TABLE  3.1 

LOAD  CELL  CHARACTERISTICS 


Strain  Guage:  Low  output 

Shear  loadings  not  tolerated 
Poor  dynamic  range 
Large  size 

Usable  for  static  applications 

Semiconductor:  High  output 

Shear  loadings  not  tolerated 
Fair  dynamic  range 
Small  size 

Usable  for  static  applications 


Piezoelectric :  Selectable  output 

Small  shear  loadings  tolerated 
Excellent  dynamic  range 
Small  size 

Not  for  static  applications 


The  strain  gauge  type  of  load  cell  was  rejected  as  a  design 
solution  primarily  on  the  basis  of  its  physical  size  and  poor  dynamic 
range.  The  semiconductor  type  of  load  cell  had  been  shown  to  be  a 
workable  solution.  However,  it  was  believed  that  the  Hexapod,  weighing 
285  pounds,  could  exceed  the  1000  pound  impact  rating  of  the  load  cell 
used  previously  if  it  were  to  slip  off  of  an  obstacle.  Semiconductor 
load  cells  with  an  impact  load  rating  of  more  than  1000  pounds  could 
not  be  located. 
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The  major  difficulty  with  the  use  of  piezoelectric  load  cells 
in  this  application  is  the  fact  that  they  require  charge  amplifiers, 
which  drift  over  time.  Hexapod  locomotion,  however,  gives  an  opportu 
nity  to  recalibrate  the  amplifier  outputs  when  the  foot  is  in  the  air 
during  transfer  phase.  This  fact  makes  the  piezoelectric  load  cell  a 
workable  solution  when  coupled  with  a  low  drift  rate  charge  amplifier 
and  this  type  of  load  cell  was  therefore  chosen  as  the  axial  force 
transducer. 

Consideration  of  the  design  factors  discussed  above  led  to  the 
selection  of  a  Kistler  Instrument  Corporation  model  912  load  cell 
as  the  axial  force  sensor.  The  relevant  specifications  for  this  de¬ 
vice  are  given  in  Table  3.2. 


TABLE  3.2 

LOAD  CELL  SPECIFICATIONS 


Manufacturer : 

Model  : 

Range  (compression): 
Resol ution : 

Overload : 

Sensitivity: 

Rigidity: 

Linearity: 

Capaci tance : 

Insulation  resistance: 
Shear  force  (mazi..um): 


Kistler  Instrument  Corp. 
No.  912 
5000  lbs. 

0.002  lbs. 

20  percent 
50  picocoulombs/1 b. 

0.2  microinches/1 b. 

+/-  1  percent 
58  picofarads 

10,000,000  megohms 
100  lbs. 


Shock  and  vibration:  10,000  g's 

5/8  in.  hexagonal  <  1/2  in 
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S  i  ze : 


With  the  piezoelectric  transducers  specified,  attention  was 
turned  to  charge  amplifier  selection.  It  was  desired  to  mount  the 
charge  amplifiers  inside  the  lower  leg  segments  of  the  Hexapod.  The 
commercially  available  units  were  too  large  to  allow  internal  mounting. 
This  fact,  together  with  the  expense  of  the  units  (about  $500.00 
each),  leg  to  a  decision  to  design  custom  amplifiers. 

The  output  of  a  piezoelectric  crystal  is  an  electrical  charge 
which  is  proportional  to  the  applied  force,  or 


Q  =  kF 


(3.1) 


where  k  is  the  sensitivity  of  the  crystal.  When  the  charge  is  allowed 
to  flow  into  an  amplifier,  the  resulting  current  is  given  by 


i  =^=  k^ 
1  dt  K  dt 


(3.2) 


If  the  amplifier  input  current  is  integrated,  the  resulting  output 
voltage  is  then  proportional  to  the  applied  force.  A  circuit  which 
performs  this  integration  is  shown  in  Figure  3.2.  If  the  circuit 
components  are  ideal,  the  circuit  transfer  function  is 


V  =  -  f  /  1  dt 


(3.3) 


where  C  is  the  value  of  the  capacitor  in  farads.  Substituting  equation 
3.2  into  3.3  gives 


V  =  -  7t  /  k  dF 


< 


or 


V  =  -  jr  F 

w 


(3.5) 


which  is  the  desired  result. 

It  was  desired  that  the  charge  amplifier  circuitry  should 
not  be  damaged  when  the  full  5000  pound  rated  load  was  applied  to  the 
load  cell.  The  feedback  capacitor  must  therefore  be  large  enough  to 
store  the  output  charge  of  the  crystal  at  full  load  without  exceeding 
the  input  voltage  rating  of  the  operational  amplifier,  which  is 
approximately  12  volts  when  operated  with  a  15  volt  supply.  The 
maximum  output  charge  of  the  crystal  is  equal  to  the  maximum  load 
multiplied  by  the  crystal  sensitivity,  or  0.25  microcoulombs.  A  0.022 
microfarad  capacitor  stores  this  amount  of  charge  with  a  12  volt  out¬ 
put,  and  is  therefore  the  value  used  in  the  circuit.  From  equation 
3.5,  this  gives  an  output  voltage  of 

V  =  2.5  ^  (3.6) 

To  minimize  capacitor  leakage,  a  polystyrene  capacitor  was  employed 
with  an  insulation  resistance  of  10,000,000  megohms. 

In  order  to  minimize  integrator  drift,  an  operational  amplifier 
with  a  low  input  bias  current  was  needed.  Referring  to  Figure  3.2, 
it  is  seen  that  any  current  flowing  into  the  inverting  input  of  the 
operational  amplifier  is  directly  subtracted  from  the  current  being 
integrated,  and  would  eventually  cause  the  integrator  to  drift  into 
saturation.  The  operational  amplifier  chosen  to  minimize  this  problem 
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was  the  National  Semiconductor  model  LH0052D.  This  J-FET  input  ampli¬ 
fier  has  a  typical  input  bias  current  of  0.5  picoamps  at  room  tempera¬ 
ture,  and  an  input  resistance  of  1,000,000  megohms.  Using  the  value 
of  capacitance  chc  en,  the  bias  current  results  in  an  output  drift  rate 
of  81  mV/ nr.  In  terms  of  force,  the  drift  rate  is  32.5  lbs. /hr.  from 
equation  3.6. 

Although  the  charge  amplification  circuitry  was  designed  to  be 
undamaged  by  a  5000  pound  input,  the  maximum  measurable  force  does  not 
need  to  be  this  large.  The  design  value  used  for  maximum  measurable 
force  was  200  pounds,  or  slightly  more  than  half  the  weight  of  the 
vehicle.  Forces  exceeding  this  value  may  cause  saturation  of  the 
final  amplification  stages. 

To  prevent  the  charge  amplifier  from  drifting  into  saturation, 
it  was  decided  to  place  a  large  resistor  in  parallel  with  the  feedback 
capacitor,  making  the  circuit  an  extremely  low-pass  filter.  The  value 
of  resistance  chosen  reflects  a  compromise  between  a  large  time  con¬ 
stant  and  a  small  maximum  voltage  drift.  It  was  decided  to  choose  the 
resistor  such  that  the  maximum  output  voltage  due  to  drift  is  equal  to 
the  voltage  due  to  the  maximum  measurable  force  intput.  A  200  pound 
input  results  in  an  output  voltage  of  500  mV,  from  equation  3.6. 

Rather  than  designing  with  a  drift  current  equal  to  the  0.5  picoamp 
input  bias  current,  it  was  decided  to  use  a  value  of  5  picoamps  to 
allow  for  current  leakage  through  the  printed  circuit  board  and  varia¬ 
tions  in  operational  arriDli  Piers.  To  satisfy  the  above  condition  the 
feedback  resistor  must  realize  a  voltage  drop  of  50C  mV  when  the  entire 


5  picoamp  drift  current  is  passing  through  it.  This  dictates  the 
selection  of  a  100,000  megohm  resistor.  Neglecting  insulation 
resistances,  the  circuit  time  constant  is  then  37  minutes. 

Due  to  the  relatively  low  output  of  the  charge  amplifier,  it  was 
necessary  to  provide  additional  voltage  amplification.  It  was  desired 
that  the  maximum  measurable  force  input  of  200  pounds  should  cause  a 
5  volt  swing  at  the  circuit  output,  leaving  sufficient  headroom  for 
the  integrator  drift.  Thus,  a  gain  of  100  was  needed  in  the  voltage 
amplification  circuit.  The  voltage  amplifier  was  constructed  in  two 
stages.  The  first  stage  is  located  in  the  vehicle  leg  near  the  charge 
amplifier,  and  raises  the  signal  to  a  level  safe  from  electrical  noise. 
The  second  stage  is  located  in  the  card  cage  at  the  rear  of  the 
vehicle.  It  low-pass  filters  the  signal  and  provides  a  manual  gain 
adjustment.  Schematics  of  the  complete  charge  amplifier  circuit  are 
shown  in  Figure  3.3. 

To  use  the  model  912  quartz  load  cell  as  a  foot  force  sensor, 
it  was  necessary  to  construct  a  special  housing  for  the  load  cell. 

The  housing,  which  was  designed  in  the  Department  of  Mechanical 
Engineering,  is  illustrated  in  Figure  3.4.  A  steel  piston  is  employed 
to  transmit  the  ground  reaction  forces  to  the  load  cell.  Teflon 
bushings  minimize  friction  between  the  piston  and  the  aluminum  case, 
and  also  provide  electrical  insulation  between  the  vehicle  leg  and  the 
load  cell.  The  point  at  which  the  load  cell  is  mounted  to  the  case  is 
insulated  with  a  high-impact  plastic  insert.  The  assembly  was  de¬ 
signed  to  withstand  the  5000  pound  rating  of  the  load  cell.  Additional 
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Figure  3.3  Piezoelectric  Charge  Amplification  Circuitry. 


details  can  be  found  in  [13].  Tests  have  been  performed  which  show 
that  the  piston  stiction  never  exceeds  10  percent  of  the  applied  side 
loading  on  the  piston  under  static  conditions.  Since  Hexapod  loco¬ 
motion  is  a  dynamic  process,  even  better  results  should  be  obtained 
in  actual  use. 

A  photograph  of  a  force  sensor  equipped  leg  is  shown  in  Figure 
3.5.  The  unit  is  shown  disassembled  in  Figure  3.6. 

3.3  Attitude  Sensors 

Two  types  of  attitude  sensors  were  investigated  for  use  on  the 
OSU  Hexapod  Vehicle:  gravitational  pendulums  and  vertical  gyroscopes. 
The  characteristics  of  each  are  listed  in  Table  3.3. 

TABLE  3.3 

ATTITUDE  SENSOR  CHARACTERISTICS 

Pendulums:  Small  size  and  weight 

Low  cost 

Immediate  operation 

Sensitive  to  lateral  acceleration 

Limited  bandwidth 

High  rel iabil ity 

Gyroscopes:  Larger  size  and  weight 

High  cost 

Requires  time  to  "spin  up"  and  erect 
Insensitive  to  lateral  acceleration 
Infinite  bandwidth 
Limited  lifetime 
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The  pendulums  are  seen  to  be  desirable  for  this  application  in 
terms  of  their  size,  reliability,  ease  of  operation,  and  cost.  At  the 
time  of  sensor  selection,  however,  it  was  not  known  whether  their 
limited  bandwidth  and  sensitivity  to  acceleration  would  represent  a 
serious  problem.  Since  a  vertical  gyroscope  does  not  exhibit  these 
shortcomings,  it  was  decided  that  one  should  be  installed  on  the 
vehicle  as  the  primary  attitude  sensor.  The  problem  of  high  cost  was 
avoided  by  the  use  of  a  surplus  gyroscope.  A  pair  of  pendulums  was 
also  installed  as  a  backup  sensor  and  for  evaluation  as  the  primary 
attitude  sensor.  The  specifications  for  the  sensors  chosen  are  given 
in  Table  3.4.  A  photograph  of  the  vertical  gyroscope  is  shown  in 
Figure  3.7,  and  the  pendulum  installation  is  shewn  in  Figure  3.8.  The 
Hexapod  is  shown  with  all  sensors  installed  in  Figure  3.9. 

3.4  Data  Link  Expansion 

All  sensor  outputs  on  the  OSU  Hexapod  Vehicle  are  transmitted  to 
the  control  computer  via  the  digital  data  link  which  was  described  in 
section  2.4.3.  It  will  be  recalled  that  the  feedback  portion  of  the 
data  link  was  originally  designed  with  a  54  channel  capability,  allow¬ 
ing  for  18  channels  each  of  rate,  position,  and  force  information. 
Since  the  attitude  sensors  require  4  channels,  it  was  necessary  to  ex¬ 
pand  the  data  link  to  allow  for  the  additional  sensors. 

It  was  decided  to  use  the  full  data  link  address  space  of  64 
channels  to  provide  for  future  sensor  additions.  The  analog  multi¬ 
plexer,  however,  was  expanded  to  a  72  channel  capability  and  is 
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TABLE  3.4 


ATTITUDE  SENSOR  SPECIFICATIONS 


GYROSCOPE  Manufacturer: 

Model  : 

Size : 

Weight : 

Erection  system: 
Erection  rate: 
Caging  time: 

Output : 

Accuracy : 

Shock  rating: 

PENDULUMS  Manufacturer: 

Model  : 

Size : 

Weight : 

Natural  frequency: 
Damping  ratio: 
Output : 

Accuracy : 

Shock  rating: 


Electronic  Specialty  Co. 
N3200 

8"  x  7.5"  x  6.75" 

6.25  lb. 

Gravity  controlled  air  jets 
3  deg. /min.  average 

1  minute  maximum 

2000  Ohm  potentiometer 
+/-  1.5  deg.  maximum  error 
30  G’s 

Humphrey,  Inc. 

CPI  7-0601 -2 

2.5"  dia .  x  1 .3"  deep 

.75  lb. 

2  Hz.  minimum 
.7  nominal 

2000  Ohm  potentiometer 
+/-  1%  (static  conditions) 
100  G's  for  10  milliseconds 
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Figure  3.9  OSU  Hexapod  Fully  Equipped  with  Sensors. 


organized  in  nine  groups  of  eight  channels  each.  Group  1  through 
group  7  are  always  active.  The  last  two  groups,  8A  and  8B,  occupy  the 
same  address  space  and  are  selected  by  a  specific  command  from  the 
control  computer.  To  provide  this  command  capability,  an  additional 
data  register  was  added  to  the  data  link  feedforward  circuitry.  This 
command  register  is  memory  mapped  to  the  control  computer  at  address 
166244  octal.  The  command  register  organization  is  illustrated  in 
Figure  3.10. 

In  addition  to  the  analog  channel  expansion  of  the  data  link,  it 
was  decided  to  include  features  to  improve  the  readability  of  the 
system.  To  reduce  the  chance  of  human  error,  a  circuit  was  constructed 
which  allows  motor  supply  power  and  instrumentation  electronics  power 
to  be  controlled  directly  by  the  computer.  Another  data  register  was 
added  to  the  data  link  feedforward  circuitry  for  communication  with 
the  power  controller.  The  power  register  is  memory  mapped  to  the  con¬ 
trol  computer  at  address  166246  octal.  The  power  register  organization 
is  illustrated  in  Figure  3.10. 

A  four  channel  digital  multiplexer  was  added  to  the  data  link 
feedback  circuitry  to  increase  the  flexibility  of  the  system.  Channel 
G  is  connected  to  the  output  of  the  analog-to-digital  converter,  and 
is  selected  in  normal  operation.  Channel  1  provides  digital  status 
information  about  the  Hexapod,  and  is  selected  when  data  link  address 
63  is  decoded.  Thus,  address  63  is  seen  externally  as  a  status  word 
and  not  as  a  sensor  output,  which  reduces  the  number  of  available 
analog  channels  to  70.  The  status  word  is  defined  in  Figure  3.10. 
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The  remaining  two  channels  of  the  digital  multiplexer  are  used  to 
implement  data  link  self-diagnostics.  Channel  2  is  used  to  perform  a 
feedforward  circuit  test,  and  is  connected  to  the  output  of  one  of  the 
feedforward  data  registers.  Channel  3  is  connected  to  the  data  link 
address  lines,  and  is  used  in  feedback  circuit  diagnostics.  Channels 
2  and  3  are  selected  by  the  command  register,  as  illustrated  in  Figure 
3.10. 

Due  to  the  modularity  of  the  original  design,  it  was  possible 
to  make  the  necessary  data  link  modifications  by  redesigning  only  the 
feedback  circuits  which  are  located  in  the  vehicle  interface.  A  block 
diagram  of  the  redesigned  feedback  circuitry  is  shown  in  Figure  3.11. 
In  addition  to  the  digital  multiplexer,  a  pipeline  register  was  added 
at  the  circuit  output  which  allows  data  conversion  to  be  performed 
while  the  last  channel  is  being  transmitted  to  the  computer  interface. 
The  pipeline  register  allows  the  data  conversion  frequency  to  be 
doubled  from  10  kHz  to  20  kHz.  Thus,  all  64  channels  are  updated  in 
3.2  milliseconds. 

3.5  Utility  Software 

To  realize  the  system  reliability  improvements  made  possible  by 
the  hardware  additions  described  in  the  last  section,  software  was 
written  which  tests  specific  Hexapod  systems  automatically.  To  pro¬ 
vide  all  Hexapod  users  with  this  capability,  subroutines  were  written 
which  can  be  linked  with  any  control  program.  These  subroutines  are 
contained  in  file  DLNK35.PAS  of  Hexapod  Control  Program  version  3.5, 
which  is  listed  in  Appendix  C. 


Figure  3.11  Functional  Block  Diagram  of  the  Feedback  Section  of  the  Digital 
Data  Link  Located  in  the  Vehicle  Interface. 


The  data  link  diagnostics  are  performed  in  two  steps.  First, 
the  feedback  portion  of  the  data  link  is  tested  independently.  The 
feedback  test  mode  is  selected  by  setting  bit  0  of  the  command  register. 
This  causes  the  data  link  address  lines  to  be  multiplexed  onto  the  data 
lines.  The  contents  of  a  location  in  the  shared  memory  can  then  be 
predicted  from  the  address  of  that  memory  location.  If  the  data  link 
is  operating  properly,  the  predicted  contents  will  agree  with  the 
actual  contents.  If  not,  a  Boolean  equivalence  operation  will  identify 
which  bits  are  incorrect.  Note  that  this  test  will  not  detect  every 
possible  malfunction  of  the  feedback  circuits,  but  does  thoroughly 
test  the  line  drivers,  transmission  cable,  and  optical  isolators. 

When  proper  operation  of  the  feedback  circuitry  is  verified,  the 
forward  path  may  be  tested.  By  setting  bit  1  of  the  command  register, 
the  output  of  the  feedforward  register  is  multiplexed  onto  the  feed¬ 
back  data  lines.  Binary  values  are  then  written  to  that  register  which 
test  all  bits  of  the  forward  path.  The  contents  of  the  shared  memory 
are  then  compared  to  the  value  written,  and  bad  bits  can  be  detected 
as  before. 

In  addition  to  data  link  diagnostics,  routines  were  written 
which  test  and  calibrate  all  sensors  on  the  vehicle.  To  test  the 
,jrce  and  attitude  sensors,  the  measured  quantities  are  output  on  a 
CRT  display,  where  they  can  be  compared  with  an  external  reference. 

The  potentiometers  are  tested  by  servoing  on  their  output  with  a  fixed 
reference  input.  The  leg  positions  are  then  visually  calibrated  by 
utiMzing  the  position  offset  adjustment  potentiometers  located  in 
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the  card  cage.  To  calibrate  the  tachometers,  their  outputs  are 
numerically  integrated  while  the  joints  are  moved  through  known 
angular  displacements.  The  tachometer  gains  are  then  adjusted  such 
that  the  rate  integral  agrees  with  the  known  displacement. 


Chapter  4 


DEVELOPMENT  OF  CONTROL  ALGORITHMS  FOR 
AUTOPILOT  IMPLEMENTATION 


4.1  Introduction 

In  this  chapter,  the  control  algorithms  necessary  to  utilize  the 
sensor  hardware  presented  in  Chapter  3  will  be  developed.  In  section 
4.2,  a  force  control  law  which  allows  active  force  accommodation  [14] 
is  presented.  The  control  law  is  identical  to  the  active  compliance 
algorithm  used  by  Klein  and  Briggs  [4],  but  the  control  law  constants 
are  specified  as  functions  of  parameters  which  predict  the  response  of 
the  entire  vehicle.  Section  4.3  deals  with  the  problem  of  force  set- 
point  specification.  A  solution  is  presented  based  on  the  pseudo¬ 
inverse  algorithm  used  by  Klein  and  Wahawisan  [15].  In  section  4.4, 
the  complete  control  structure  of  the  compliance  servo  is  designed. 

An  algorithm  for  controlling  the  body  attitude  of  the  Hexapod  is 
developed  in  section  4.6.  These  algorithms  together  constitute  the 
software  portion  of  the  autopilot  design. 

4.2  Force  Control  Law  Design 

The  problem  of  designing  a  control  law  to  utilize  the  foot 
force  information  is  complicated  by  the  fact  that  the  Hexapod  is 
operating  in  a  relatively  unstructured  environment.  The  control  law 
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must  combine  force  information  along  with  position  and  velocity  infor¬ 
mation  in  a  manner  which  enhances  the  ability  of  the  Hexapod  to  nego¬ 
tiate  rough  terrain,  even  if  little  is  known  about  a  particular  terrain 
point.  The  control  law  must  therefore  be  flexible  enough  to  operate 
successfully  on  terrain  points  with  a  wide  range  of  values  for  eleva¬ 
tion,  compliance,  coefficient  of  friction,  etc. 

The  state  of  a  foot  shall  be  defined  by  vectors  describing  the 
position,  velocity,  and  applied  force  of  that  foot  along  each  axis  of 
the  body  coordinate  system. 


SXA  "  ^XA  XA  fXA^ 
^YA  =  C*A  ^A  fYA^ 


5  tZA  ^A  fZA^ 


(4.1) 

(4.2) 

(4.3) 


The  subscript  'A'  denotes  an  actual  value  as  opposed  to  a  desired  value. 

The  output  of  the  force  sensing  circuitry  is  a  vector  with  com¬ 
ponents  along  each  axis  of  the  foot  coordinate  system,  which  is 
described  in  Appendix  A.  In  general,  the  foot  coordinate  system  is 
physically  rotated  from  the  body  coordinate  system.  The  force  infor¬ 
mation  can,  however,  be  mathematically  transformed  into  the  body 
coordinate  system  by  the  relation 

Fg  =  R(5)  Ff  (4.4) 


47 


where  is  the  force  vector  expressed  in  the  foot  coordinate  system, 
7g  is  the  force  vector  expressed  in  the  body  coordinate  system,  and 
R(0")  is  the  required  rotation  matrix.  The  rotation  matrix  is  derived 
in  Appendix  A. 

The  control  law  design  is  simplified  by  considering  the  state  of 
a  foot  in  only  one  spatial  dimension.  The  z-axis  is  chosen  for  dis¬ 
cussion;  the  following  development  is,  however,  equally  applicable  to 
the  x  and  y  axes. 

Suppose  that  a  desired  state  has  been  specified  for  a  foot,  and 
is  given  by 

SZD  S  ^-ZD  fZD^ 


where  the  subscript  ‘D1  denotes  a  desired  value.  Since  the  inner  loop 
of  the  existing  Jacobian  servo  is  a  rate  loop,  it  is  reasonable  to 
specify  a  rectilinear  velocity  error  signal  ey  such  that 


ev  kpz(W  +  kvz(W  +  kfz(fZD"W  * 


(4.6) 


To  simplify  analysis  of  this  control  law,  it  shall  be  assumed  that  the 
rate  servo  loop  is  ideal,  and  therefore  that  the  error  signal  ey  is 
always  zero.  The  ideal  rate  servo  assumption  is  used  in  much  of  this 
work.  The  actual  servo  model  will  be  included  in  a  later  section,  and 
it  will  be  shown  that  the  results  obtained  using  this  assumption  are 
valid.  Equation  4.6  then  reduces  to 


DZ '  Zu 


_  7 


0  +kvz<ZD-ZA> 


kfz(fZD'fW 


(4.7) 
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For  a  given  state  setpoint  S^q,  equation  4.7  constrains  the 
actual  state  to  lie  within  a  two-dimensional  space.  This  is 
illustrated  in  Figure  4.1,  where  plane  I  represents  this  two-dimension¬ 
al  space.  The  plane  passes  through  point  D,  which  represents  the 
desired  state  S^.  Note  that  the  actual  state  is  not  forced  to  point 
D,  but  rather  can  exist  anywhere  on  plane  I. 

It  is  reasonable  to  expect  that  the  external  environment  will 
impose  additional  constraints  on  the  system.  To  illustrate,  consider 
that  a  foot  in  support  phase  is  in  contact  with  the  terrain.  The 
state  of  a  given  terrain  point  can  be  defined  in  the  same  manner  as 
the  state  of  a  foot.  Remaining  in  the  body  coordinate  system  and 
using  the  subscript  'T1  to  denote  a  terrain  point  yields 

Sn  =  [ZT  ZT  fn]T  (4.8) 

Any  terrain  point  will  exhibit  some  amount  of  compliance.  In 
addition,  the  passive  compliance  of  the  hexapod  structure  can  be 
lumped  with  the  terrain  compliance.  For  the  sake  of  discussion,  this 
compliance  will  be  assumed  to  be  linear,  and  can  be  described  by 

fZT  ”  VW  *  »T<V  (4-9) 

where  k^  is  the  terrain  spring  constant,  Zq  is  the  terrain  height  with 
no  force  applied,  and  cty  is  the  terrain  viscous  damping  constant.  Note 
that  for  a  constant  Zq,  the  vehicle  coordinate  system  is  at  a  fixed 
height  above  the  nominal  terrain  surface,  and  thus  the  hexapod  mass 
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does  not  undergo  acceleration. 

When  a  foot  is  in  support  phase,  it  is  assumed  to  be  in  firm 
contact  with  the  terrain;  i.e.,  the  relative  velocity  between  the  foot 
and  the  terrain  point  is  zero.  If  the  foot  force  f^A  and  the  ground 
reaction  force  f^T  are  defined  is  opposite  directions,  then  their 
values  must  be  exactly  equal.  The  state  of  a  foot  in  support  phase  is 
thus  exactly  equal  to  the  state  of  the  supporting  terrain  point,  or 


(4.10) 


Substituting  into  equation  4.9  then  yields 

fZA  ’  VW  +  “TJA  ’  (4-"> 

Equation  4.11  is  the  constraint  imposed  on  the  system  by  the  external 
environment,  and  is  represented  by  plane  II  in  Figure  4.1.  Plane  I 
and  plane  II  intersect  at  line  BC.  The  actual  system  state  is  there¬ 
fore  constrained  to  remain  within  the  one-dimensional  space  repre¬ 
sented  by  BC  if  the  constant  vehicle  height  condition  is  imposed. 

Note  that  the  previous  discussion  considered  only  one  possible 
constraint  on  the  system.  When  the  foot  is  not  in  contact  with  the 
terrain,  the  system  is  constrained  to  lie  on  the  plane  f^A  =  0. 

The  environmental  constraints  on  the  system  will  in  general  be  time- 
varying  and  nonlinear. 

Additional  insight  into  the  control  law  can  be  obtained  by 
modeling  it  as  a  physical  system.  Equation  4.7  can  be  solved  for  f 
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giving 


Note  that  the  block  ' M '  is  not  part  of  this  model,  but  is  included  for 
later  use.  The  unfamiliar  Iq  term  in  equation  4.13  is  due  to  the 
sliding  joint  between  links  L-|  and  l_2  which  allows  the  overall  length 
to  be  adjusted  without  exercising  the  spring-damper  system.  Note  that 
jIq  is  a  constant,  giving 


Z 


0 


(4.19) 


from  equation  4.18.  Thus,  if  L-j  and  Lj  comprise  a  rigid  link,  Zq  is 
seen  to  be  zero  and  equation  4.13  reduces  to  standard  form. 

The  physical  system  model  of  Figure  4.2  is  helpful  in  under¬ 
standing  the  characteristics  of  the  force  control  law,  and  will  be 
referred  to  in  later  sections.  It  also  illustrates  why  the  control 
law  of  equation  4.6  is  referred  to  as  an  'active  compliance'  algorithm. 

Before  investigating  the  effects  of  a  time-varying  state  set- 
point  it  must  be  emphasized  that  the  desired  velocity  is  not 

necessarily  equal  to  the  derivative  of  the  desired  position  Zp.  To 
avoid  notation  problems,  the  temporary  change  of  variables 


r  '-I  A 
rD  -  Ln  -  vdt'o 


(4.20) 


shall  be  made . 

Consider  the  case  where  rQ  =  0  and  Zp  varies.  The  Zp  term  in 
equation  4.12  is  then  zero,  and  the  equation  has  the  form  of  a  simple 
spring-damper  system.  With  reference  to  the  physical  model,  this 
means  tnat  the  sliding  joint  is  locked.  Thus  any  change  in  is 


5d 


In  words,  the  actual  rate  is  exactly  equal  to  the  desired  rate  if  the 
following  conditions  are  met:  the  desired  rate  is  the  derivative  of 
the  desired  position,  the  force  error  is  constant,  and  the  initial 
rate  error  is  zero.  It  is  seen  intuitively  that  for  a  non-zero  initial 
rate  error,  the  rate  error  approaches  zero  exponentially. 
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It  is  informative  to  interpret  the  implications  of  equation  4.21 
in  terms  of  the  physical  model.  Reverting  to  the  original  notation, 
equation  4.25  becomes 


(4.26) 


Equations  4.17  and  4.19  may  be  used  to  write  equation  4.26  in  terms  of 
the  physical  model  parameters,  giving 


(4.27) 


Referring  to  Figure  4.2,  it  is  seen  that 
ZA  =  Z£  +  «,  , 


(4.28) 


giving 

Zfl  •  Zt  +  i  .  (4.29) 

Combining  4.27  and  4.29  gives 

1=0.  (4.30) 

Thus,  all  system  motion  occurs  at  the  sliding  joint  and  the  spring- 

damper  system  is  not  excited  given  that  equation  4.21  is  imposed,  the 

force  error  is  constant,  and  the  initial  velocity  error  is  zero. 

The  force  control  law  design  can  now  be  completed  by  specifying 

the  constants  k  ,  k  ,  and  kr  in  terms  of  desired  system  parameters, 
p  v  I- 

For  the  purposes  of  vehicle  suspension  design,  the  obvious  ones  are 
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the  spring  constant  kg  and  the  system  damping  ratio  £.  The  approach 
used  to  solve  for  the  constants  is  to  write  the  system  dynamic  equations 
using  the  physical  model  and  then  transform  between  physical  model 
parameters  and  control  law  constants.  To  study  the  system  dynamics, 
the  mass  of  the  hexapod  supported  by  the  leg  must  be  included  in  the 
physical  model,  and  is  represented  by  the  block  ' M ‘  in  Figure  4.2. 

For  the  purpose  of  studying  system  dynamics,  the  sliding  joint  is 
assumed  to  be  locked. 

Applying  the  laws  of  physics  to  Figure  4.2,  one  can  obtain 

-fZA  =  -M(‘Vg)  {4-31) 

where  ' g *  is  the  acceleration  of  gravity.  Summing  all  forces  on  the 
sliding  link  assembly  gives 

~fZA  '  ks(W  "  °^W  =  °  •  (4-32) 

Combining  equations  4.31  and  4.32  gives 

k$(ZA-Z0)  +  ct(ZA-ZQ)  +  M(ZA-g)  =  0  .  (4.33) 

With  the  sliding  joint  locked,  Zq  is  zero  and  ZQ  is  constant,  as  is 
the  acceleration  of  gravity.  Thus,  the  homogeneous  part  of  equation 
4.33  is 

ksZAt;VM2A*°  '  (4'34) 
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Using  differential  operator  notation  on  equation  4.34  gives 


<°2  *  S  D  *  -#>ZA  ’  0 


(4.35) 


Comparing  this  to  a  general  second-order  system 


(D2  +  2^0  +  a)2)ZA  =  0 


(4.36) 


it  is  apparent  that 

,/T 

wn  V  M 
and 

9r  -  a 

2^n  '  M  • 

Combining  equations  4.37  and  4.38  gives 


(4.37) 


(4.38) 


or 


1/* 


1 

2^Fm 


(4.39) 


(4.40) 


The  physical  model  parameters  can  now  be  replaced  with  control  law 
constants.  Substituting  equation  4.15  into  4.40  results  in 


v 


1 


2 

s 


(4.41  ) 
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Substituting  equation  4.14  into  4.41  gives 


(4.42) 

(4.43) 

Since  the  control  law  of  equation  4.6  includes  three  constants  with 
which  to  specify  two  independent  parameters,  one  of  the  constants  can 
be  specified  arbitrarily.  Thus,  the  following  condition  is  imposed. 

kv  =  1  (4.44) 

Equations  4.41,  4.43,  and  4.44  complete  the  control  law  design. 

The  most  general  requirement  for  a  control  law  utilizing  force 
feedback  was  that  it  must  improve  the  rough  terrain  capability  of  the 
hexapod.  The  linear  control  law  just  developed  is  expected  to  provide 
benefits  similar  to  those  of  ordinary  passive  suspensions;  in  particu¬ 
lar,  it  causes  all  legs  in  support  phase  to  maintain  contact  with  the 
ground  and  support  a  load  approximately  equal  to  a  force  setpoint 
specified  by  a  higher  level  control  algorithm.  The  control  law  is 
expected  to  have  several  advantages  over  a  passive  system.  First,  the 
spring  stiffness  and  damping  can  be  changed  with  no  hardware  modifi¬ 
cations,  or  could  even  be  changed  dynamically.  Secondly,  the  particu¬ 
lar  leg  geometry  used  by  a  vehicle  does  not  affect  the  algorithm,  since 


or 


k  /k 
P  s  = 


2  ?/yi 


v 
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the  algorithm  is  defined  in  body  coordinates.  Thirdly,  the  major 
compliant  axis  could,  through  the  use  of  another  rotation  matrix,  be 
maintained  parallel  to  the  gravitational  vector  at  all  times.  This 
would  allow  the  use  of  "soft"  spring  constants  even  with  the  body  at 
extreme  pitch  and  roll  angles  without  affecting  the  vehicle's  stability 
margin . 

4.3  Force  Setpoint  Specification 

Prior  to  implementing  the  control  law  of  equation  4.6,  the 
desired  system  state  S^  must  be  defined.  The  parameters  Z D  and  ZQ  are 
specified  by  the  foot  trajectory  planning  algorithm  and  by  the  body 
attitude  regulation  algorithm  of  section  4.5.  It  remains  to  specify 
the  force  setpoint  "F  . 

For  the  purpose  of  force  setpoint  specification,  the  terrain  is 
modeled  as  a  flat,  non-compl iant  surface.  The  problem  then  reduces  to 
the  determination  of  ground  reaction  forces  which  cause  the  vehicle 
body  to  maintain  static  equilibrium.  Throughout  this  work,  the  vehicle 
body  is  assumed  to  be  perpendicular  to  the  gravitational  vector,  i.e., 
gravitational  loading  causes  force  components  parallel  to  the  body 
coordinate  z-axis  only.  Since  acclerations  are  quite  small  on  this 
vehicle,  zero  force  setpoints  can  be  specified  along  the  x  and  y  com¬ 
ponents  of  the  body  coordinate  system  when  active  compliance  is  enabled, 
givi ng 


XD 


(4.45) 
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and 


fyD  -  0  •  (4.46) 

In  addition,  legs  in  transfer  phase  are  not  in  contact  with  the  ground, 
giving 


=  0  ,  transfer  phase  .  (4.47) 

For  the  vehicle  body  to  maintain  static  equilibrium,  three  con¬ 
ditions  must  be  satisfied. 

1.  Sum  of  moments  about  the  x-axis  must  equal  zero. 

2.  Sum  of  moments  about  the  y-axis  must  equal  zero. 

3.  Sum  of  vertical  ground  reaction  forces  must  equal  the 
total  weight  of  the  vehicle. 

Expressed  as  equations,  these  conditions  become 


y  m  =  o  , 

i  x 

(4.48) 

o 

ii 

>> 

s: 

U — J*r- 

(4.49) 

and 

^total  ’ 

(4.50) 

for  leg  i  in  support  phase. 

There  may  be  up  to  six  legs  ir.  support 

phase,  so  in  general  the  above  system  of  equations  is  underspecified. 

A  pseudo-inverse  solution  to  these  equations  may  be  used  to  optimize 
the  setpoints  with  respect  to  a  minimum  sum  of  squares  of  force.  This 
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type  of  solution  has  been  used  previously  by  Klein  and  Wahawisan,  who 
solved  the  system  of  equations  numerically  in  a  real-time  computer  pro¬ 
gram.  In  Appendix  B,  it  is  shown  that  a  closed-form  analytic  solution 
can  be  found  for  the  system.  The  solution  is  given  by  equations  B.17, 
B.18,  B .20,  and  B.26. 

4.4  Force  Control  Law  Implementation 

In  implementing  the  force  control  law,  it  is  desired  that  as  much 
as  possible  of  the  Jacobian  control  structure  which  is  currently  in  use 
be  retained.  Figure  4.3  is  a  block  diagram  which  implements  the  force 
control  law  in  body  coordinates  and  then  uses  a  Jacobian  control  struc¬ 
ture  to  convert  between  body  and  joint  coordinates.  The  diagram  shows 
the  entire  control  structure  for  one  leg.  The  parameters  are  defined 
in  Table  2.1  and  Table  4.1. 

The  implementation  of  Figure  4.3  satisfies  the  control  law,  but 

requires  the  use  of  the  forward  Jacobian  matrix,  resulting  in  increased 

complexity.  However,  the  diagram  can  be  reduced  to  the  form  of  Figure 

4.4  by  making  the  following  observations.  Recall  that  when  solving  the 

control  law  for  k  ,  kr,  and  k  ,  it  was  chosen  to  set  k  =1.  Thus, 
p  f  v  v 

K  =  I  ,  and 
v 

j_1(0)kv  J(0)e  =  j_1 (e) I  J(e)s  =  e  .  (4. si) 

This  allows  the  rate  feedback  loop  to  be  reduced  as  shown  in  Figure  4.4. 


Comparison  of  this  figure  with  the  existing  structure  (figure  2.8)  shows 
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Table  4.1 


DEFINITIONS  OF  ADDITIONAL  CONTROL  PARAMETERS 
FOR  FORCE  CONTROL  LAW  IMPLEMENTATION 


tfXD  fYD  fZD^ 


(desired  force  expressed 
in  body  coordinates) 


=  [fv 


(actual  force  expressed 
in  body  coordinates) 


Ff  5  ^fXf  fYf  fZf^ 


(actual  force  expressed 
in  foot  coordinates) 


E-  =  [e  e  el 
x  x  y  z  J 


Kv, 


vx 

0 

0 


vy 

0 


0 

0 

cvz 


(rectilinear  rate  error  ex¬ 
pressed  in  body  coordinates) 


(rate  gain  matrix) 


(force  gain  matrix) 


R(8)  =  foot  force  rotation  matrix. 


Compensator  =  a  functional  block  specifying  the  relationship 

between  joint  rate  error  and  actuator  input  voltage. 


that  the  force  control  law  has  been  implemented  with  the  simple 
addition  of  a  force  feedback  loop  to  the  existing  structure.  The 
parameters  are  defined  as  in  Table  2.1  and  Table  4.1. 

4.5  Non-Ideal  Servo  Effects  and  Compensator  Design 

To  study  the  effects  of  a  non-ideal  servo  on  system  performance, 
the  control  block  diagram  of  Figure  4.4  can  be  reduced  to  a  single 
spatial  dimension  and  linearized  about  an  operating  point.  The  oper¬ 
ating  point  chosen  is  the  normal  or  "square"  position,  which  is  shown 
in  Figure  A.l .  In  this  position,  the  motion  along  each  Cartesian  axis 
is  controlled  primarily  by  a  single  joint  actuator.  Foot  motion  along 
the  body  coordinate  x-axis  is  completely  decoupled  in  the  normal  posi¬ 
tion,  thus  the  simplified  control  model  uses  this  axis  only. 

Referring  to  Figure  A.l,  it  is  seen  that 

X  =  ( 2^  +  £4)  sin  (4.52) 

where  X  is  the  distance  from  the  origin  of  the  hip  coordinate  system 
to  the  foot  along  the  body  coordinate  x-axis,  and  the  offset  has 
been  neglected.  If  i  is  defined  as 

1  5  ^  ,  (4-53) 

linearizing  the  system  yields 

X  =  i*  (4.54) 

and 
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X  =  Zip  . 


(4.55) 


With  re  °ct  to  Figure  4.4,  the  kinematic  equations  reduce  to  simple 
multiplication  by  £,  and  the  inverse  Jacobian  matrix  becomes  the  scalar 
1 /£.  The  force  feedback  loop  is  eliminated,  since  the  relationship 
between  input  voltage  and  applied  force  is  not  well  defined.  The  re¬ 
duced  model  is  intended  to  study  system  dynamics  only;  the  reference 
inputs  may  therefore  be  eliminated.  The  hexapod  itself  is  reduced  to 
the  single  ip-axis  joint  actuator.  A  simple  linear  transfer  function 
which  has  been  found  to  give  accurate  predictions  of  actuator  perfor¬ 
mance  is 


>MD)  _  .14 

vtoT  D(  D+3 ) 


(4.56) 


When  all  of  the  preceeding  simplifications  are  made  to  Figure  4.4,  the 
block  diagram  of  Figure  4.5  results. 

The  control  system  external  to  the  actual  actuator  hardware  is 
implemented  on  a  digital  computer;  the  system  is  therefore  a  discrete 
time  system.  It  shall  be  assumed,  however,  to  be  a  continuous-time 
system  for  the  purpose  of  analysis.  To  insure  the  validity  of  this 
assumption,  all  time  constants  will  be  chosen  to  be  at  least  four  times 
as  long  as  the  time  between  servo  computations.  The  control  program 
executes  at  approximately  40  nz,  thus  the  time  constants  are  to  oe 
chosen  such  that 

t  >1/10  sec  .  (4.57) 
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The  compensator  block  provides  opportunities  to  minimize  the 
effects  of  servo  nonlinearities  and  disturbance  inputs.  Klein  and 
Briggs  [4],  for  example,  used  "variable  structure  system"  control  to 
eliminate  motor  stalling.  A  linear  gain,  however,  is  known  to  result 
in  satisfactory  performance  and  is  therefore  chosen  for  the  sake  of 
simplicity.  When  a  compensator  gain  G-j  is  substituted  for  the  compen¬ 
sator  block,  Figure  4.5  can  be  reduced  to  the  form  shown  in  Figure  4.6, 
which  shall  be  used  for  the  system  analysis.  If  the  substitution 

G  =  .14  G1  (4.58) 

is  made,  the  system  can  be  described  mathematically  by 


(4.59) 


The  eigenvalues  of  the  system  are  the  *'ts  of  the  characteristic 
equation 

a2  +  (3+G) a  +  Gkp  =  0  .  (4.60) 

Since  kp  is  already  specified  by  equation  4.43,  only  one  eigenvalue 
can  be  assigned  by  specifying  a  value  for  G.  The  associated  time  con¬ 
stant  should  be  as  small  as  possible  to  avoid  invalidating  the  ideal 
servo  assumption.  The  system  should  thus  have  two  real  eigenvalues, 
giving  a  characteristic  equation  of  the  form 

( -+a)(\+b)  =  0  (4.61) 
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Figure  4.6.  One-dimensional  Control  Structure  with  Simple  Compensator. 


X  +  (a+b)A  +  ab  =  0 


(4.62) 


Equating  the  coefficients  of  equations  4.60  and  4.62  gives  the  system 
of  equations 


3+G  ~  a+b 


Gk  =  ab 
P 


(4.63) 


(4.64) 


Solving  equations  4.63  and  4.64  yields 


k  (b-3) 

1=^T 


(4.65) 


(4.66) 


For  purposes  of  comparison,  consider  again  the  ideal  servo 
assumption.  If  the  inner  rate  loop  of  Figure  4.5  were  ideal,  then  the 
actual  velocity  would  equal  the  commanded  velocity,  or 


(4.67) 


From  the  block  diagram,  equation  4.67  results  in 


*A  =  "Va  ‘ 


(4.68) 


Solving  equation  4.68  yields  the  ideal  system  response 


-k  t 

<1<A  -  V  • 


(4.69) 


Thus,  the  ideal  system  is  seen  to  be  first  order  with  an  eigenvalue  at 


Aideal  =  ‘kp  * 


(4.70) 


This  eigenvalue  has  a  special  significance  with  respect  to  the  active 
compliance  algorithm.  Consider  equation  4.12,  which  is  the  force  con¬ 
trol  law  of  equation  4.7  rearranged  to  describe  a  spring-damper  system. 
The  homogeneous  response  may  be  obtained  by  solving  the  equation  with 
zero  setpoints  and  zero  actual  force,  giving 


k  k 

JL  7 _ 3L  7 

kp  k 


(4.71) 


k 

JtL  i 

kv  A  • 


(4.72) 


Making  use  of  equation  4.44,  the  solution  of  4.72  is 


7  =  7  e  P 

Lk  L0e 


(4.73) 


Thus,  the  ideal  system  pole  gives  the  homogeneous  response  of  the 
spring-damper  system. 


72 


The  actual  system  eigenvalues  can  now  be  compared  with  the  ideal 
system  eigenvalue.  From  equation  4.61,  the  actual  system  has  eigen¬ 
values  at 

X-|  =  -a 
and 

L  s  -b  . 

Taking  the  limit  of  equation  4.65  results  in 

lim  a  =  k  (4.76) 

b-x»  ^ 

or 

lim  A,  =  -k  .  (4.77) 

b~>  1  P 


(4.74) 

(4.75) 


Comparing  equations  4.70  and  4.77,  it  is  seen  that  if  the  second  eigen 
value  Ag  1s  verV  large,  the  actual  system  response  is  the  same  as  the 
ideal  system  response.  The  value  of  A£  is  limited  by  equation  4.57, 
however,  giving 


-10 


(4.78) 


or 


b  =  10  . 


(4.79) 


There  now  exist  analytic  expressions  for  all  system  gains  as 
functions  of  the  spring  stiffness  and  damping  parameters  k  and  *. 


The  values  used  to  obtain  the  experimental  results  presented  in  Chapter 
5  are 

k$  =  8  lb/in  (4.80) 

and 

4=2.  (4.81) 


The  largest  mass  which  will  be  supported  by  one  leg  is  1/2  of  the  total 
mass  of  the  hexapod.  The  effective  mass  is  then 


M 


285  1 bm 
2 


.368 


1 bf  sec^ 
in 


(4.82) 


Substituting  these  values  into  equations  4.41,  4.43,  and  4.44  yields 

kp  »  .146  (4.83) 

and 

kp  =  1  .166  .  (4.84) 

The  system  pole  A-j  associated  with  the  spring  damping  is  given  by 
equations  4.74,  4.65,  and  4.79  as 


a,  =  -.924  . 


(4.85) 


Comparing  this  value  with  equation  4.70  shows  that  there  exists  a 
twenty  percent  difference  in  the  eigenvalues  predicted  by  the  ideal  and 
the  non- ideal  servo  models.  Thus,  the  force  control  law  is  expected 
to  approximate  a  spring-damper  system  with  reasonable  accuracy.  If 
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more  accuracy  is  needed,  a  compensator  design  using  integral  error 
feedback  can  be  employed  which  reduces  the  eigenvalue  discrepancy  to 
under  five  percent.  This  compensator,  however,  has  not  yet  been 
tested  experimentally. 

To  finish  the  design,  the  system  forward  gain  is  obtained  from 
equation  4.66  as 

G  =  7.92  .  (4.86) 

The  compensator  gain  is  obtained  from  equation  4.58,  giving 

G1  =  56.6  .  (4.87) 

The  constants  specified  in  equations  4.83  and  4.84  are  for  use  as  z- 
axis  parameters.  The  x  and  y  axis  parameters  are  determined  similarly. 
The  compensator  gain  of  equation  4.87  is,  however,  used  for  all  actua¬ 
tors  . 

4.6  Attitude  Cont'ol 

The  primary  function  of  the  active  compliance  algorithm  is  to 
provide  a  suspension  system  for  the  hexapod;  it  causes  all  legs  in 
support  phase  to  maintain  contact  with  an  uneven  surface  and  to  support 
a  proportionate  amount  of  the  vehicle  weight.  To  insure  that  the 
vehicle  can  accommodate  large  terrain  height  variations  under  the 
supporting  feet,  fairly  "soft"  spring  constants  were  chosen.  This, 
however,  causes  the  vehicle  body  attitude  to  be  less  restrained  than 
is  desirable,  i.e.,  the  body  can  pitch  and  roll  excessively.  Even 
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with  "firm"  spring  constants,  the  terrain  itself  could  cause  the 
vehicle  to  enter  an  undesirable  attitude.  To  avoid  those  problems 
and  to  generally  increase  the  control  capabilities  of  the  hexapod,  an 
algorithm  for  attitude  control  is  seen  to  be  desirable. 

In  Chapter  3,  hardware  was  implemented  which  measures  the  angular 
displacement  of  the  vehicle  body  from  the  vertical  with  respect  to  both 
the  pitch  and  roll  axes.  As  was  stated  earlier,  the  vehicle  body  is 
assumed  to  be  maintained  in  a  level  attitude  through  this  work.  It  is 
therefore  desired  that  the  attitude  control  system  maintain  the  pitch 
and  roll  displacements  as  close  as  possible  to  zero.  To  accomplish 
this,  a  linear  feedback  control  law  may  be  implemented  with  a  zero 
reference  input.  The  attitude  enor  is  then  simply  the  output  of  the 
vertical  sensor.  The  mechanism  by  which  body  attitude  is  changed  is 
the  displacement  of  feet  in  support  phase  with  respect  to  the  body 
coordinate  system.  Figure  4.7  shows  the  relationship  between  foot 
displacement  and  body  attitude  in  two  dimensions.  The  actual  body 
pitch,  is  '‘"fined  as  the  body  angular  displacement  (positive 
counterclockwise)  from  the  vertical  about  the  positive  y  axis  and  is 
expressed  in  radians.  Figure  A  shows  the  vehicle  with  an  attitude 
error,  while  Figure  B  shows  the  vehicle  after  the  attitude  has  been 
corrected.  The  z-axis  position  displacement  of  leg  i  necessary  to 
accomplish  the  correction  is 

AZi  -  7_:  -  1.  =  [Zi  -  Xi.sin(d>2-g>1  )]  -  Z.  (4.88) 


or 
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where  <f>  is  defined  as  the  angle  between  the  vehicle  body  and  the 
terrain.  The  term  is  seen  to  have  the  same  value  as  ya  in 

Figure  A.  Since  the  feedback  control  will  be  forcing  ya  to  be  near 
zero,  the  approximation 

sin  Ya  ~  YA  (4.90) 

may  be  used.  Tnen  the  relation 

AZi  =  -XiYa  (4.91) 

can  be  used  in  the  control  algorithm  to  convert  from  attitude  error  to 
position  displacement. 

The  actual  body  roll,  6A»  is  now  defined  as  the  body  angular 
displacement  (positive  counterclockwise)  from  the  vertical  about  the 
positive  x  axis.  For  a  body  roll  error,  the  foot  displacement  neces¬ 
sary  for  correction  is  found  to  be 

AZ.j  -  Yi6A  .  (4.92) 

In  general,  the  vehicle  may  exhibit  pitch  and  roll  errors  simultaneous 
ly.  It  can  be  shown  that  for  small  errors,  the  foot  correction  neces¬ 
sary  for  an  error  about  a  given  axis  is  independent  of  the  error  about 
the  other  axis,  i  . e . ,  the  axes  are  decoupled.  The  general  expression 
for  foot  displacement  is  then 


(4.93) 


AZi  =  "Xi^A  +  VA  • 

It  should  be  noted  that  a  change  in  the  vehicle  body  attitude 
necessitates  a  change  not  only  in  the  z-coordinate  of  the  foot,  but 
also  in  the  x  and  y  coordinates.  Since  the  attitude  is  expected  to 
remain  almost  constant,  however,  these  x  and  y  displacements  are 
assumed  to  be  zero.  If  it  were  desired  to  implement  variable  attitude 
setpoints,  a  rotation  matrix  could  be  found  which  would  give  exact 
values  for  displacements  in  all  dimensions.  Equation  4.93  is  actually 
a  small-signal  linearization  of  that  matrix. 

The  required  foot  motions  can  be  effected  by  varying  the  refer¬ 
ence  inputs  to  the  compliance  servo.  Referring  to  the  physical  model 
of  Figure  4.2,  it  is  seen  that  a  straightforward  mechanism  for  imple¬ 
menting  the  attitude  correction  term,  AZ. ,  is  to  simply  adjust  the 
sliding  joint  without  directly  invoking  any  spring-damper  response. 
Recall  that  this  is  achieved  by  specifying  the  reference  inputs  such 


that 


ZD  ■  V 


(4.94) 


In  a  hexapod  control  scheme  without  attitude  feedback,  the  compliance 
servo  inputs  are  specified  directly  by  the  foot  trajectory  planning 
algorithm;  these  inputs  are 


and 


(4.96) 
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where  the  parameters  with  the  superscript  ’ T '  are  the  outputs  of  the 
foot  trajectory  algorithm  and  satisfy  equation  4.94.  If  attitude  con¬ 
trol  is  to  be  implemented,  the  control  loop  must  be  able  to  access  the 
compliance  servo  inputs.  The  following  equations  provide  that  access. 


ZD  ■  Z5  +  ZD 


(4.97) 


•  *T  *A 

7  =  7  +  l 

D  D  D  ‘ 


(4.98) 


Here  the  superscript  'A'  denotes  parameters  generated  by  the  attitude 
control  algorithm. 

A  -A 

To  close  the  attitude  control  loop,  the  parameters  ZQ  and  ZQ 
must  be  specified  in  terms  of  the  attitude  correction  displacement  , 
and  they  must  be  specified  such  that  they  satisfy  equation  4.94.  If 
ideal  rate  servos  are  assumed,  then  the  compliance  servo  for  each  leg 
is  ideal,  and  therefore  the  entire  vehicle  can  be  modeled  as  an  ideal 
system,  such  that 


YA  =  yD 


(4.99) 


5  A  '5d 


(4.100) 


The  control  laws 


7D  = "kAvA 


(4.101 ) 


V  'Va 


(4.102) 


i 


then  cause  the  pure  exponential  responses 


yA  =  V 


-kA(t-t0) 


(4.103) 


6a  =  V 


-kA(t-t0) 


(4.104) 


if  the  hexapod  mass  is  neglected,  and  the  system  time  constant  is  seen 
to  be 


tA  "  ,/kA  • 


(4.105) 


The  control  laws  of  equations  4.101  and  4.102  cannot  be  implemented  as 
written,  however,  since  there  exists  no  direct  attitude  inputs. 
Differentiating  equation  4.93  gives 


AZi  -  -XiYA  ♦  Yf«A 


(4.106) 


Since  this  relation  was  obtained  from  geometrical  considerations  only, 
it  is  valid  as  a  transformation  between  desired  rates  as  well  as  actual 
rates.  Then 


ZS  =  -Vo  +  Vo  • 


(4.107) 


Substituting  equations  4.101  and  4.102  into  4.107  gives 


*D  =  XiVW 


(4.108) 


*b  =  V  V  -  v>  • 


(4.109) 


Equation  4.109  specifies  the  foot  rate  required  to  implement  the 
control  laws  4.101  and  4.102.  The  position  input  to  the  compliance 
servo  can  be  obtained  by  numerically  integrating  the  rate  input,  giving 


(4.110) 


where  t  is  the  time  at  which  the  foot  entered  support  phase.  Equation 
4.110  insures  that  equation  4.94  is  satisfied,  and  therefore  the  active 
spring  damping  is  not  invoked. 

To  complete  the  design,  a  value  for  the  attitude  system  pole 
must  be  chosen.  Recall  that  the  compliance  servo  has  two  poles,  one 
of  which  provides  the  active  spring  damping,  and  another  which  was 
assigned  at  10  radians/second.  The  attitude  control  loop  does  not 
excite  the  pole  associated  with  the  damping  due  to  the  fact  that 
equation  4.94  is  satisfied.  For  the  ideal  servo  assumption  to  be 
valid,  however,  the  attitude  control  pole  must  be  smaller  than  the 
other  servo  pole.  Fast  response  is  desired  of  the  attitude  control 
loop,  so  the  attitude  pole  is  specified  at 

kA  =  8  .  (4.111 ) 


A  block  diagram  of  the  attitude  control  system  is  s:'own  in  Figure  4.8. 
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Figure  4.8. 


Chapter  5 

EXPERIMENTAL  RESULTS  OF  AUTOPILOT  PERFORMANCE 
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5.1  Introduction 

Experimental  results  are  presented  in  this  chapter  which  document 
the  performance  of  the  control  algorithms  developed  in  Chapter  4.  Modi¬ 
fications,  based  on  experimental  results,  to  the  basic  control  algor¬ 
ithms  are  also  explained  and  documented.  The  algorithms  are  tested 
both  individually  and  in  combination. 

Hexapod  Control  Program  version  3.0  was  used  as  the  base-line 
program  for  this  work.  The  force  accommodation  and  attitude  control 
algorithms  were  added  to  the  program,  with  software  switches  allowing 
them  to  be  bypassed  if  desired.  The  resulting  program  is  Hexapod 
Control  Program  version  3.4.  Appendix  C  is  a  listing  of  this  program. 

The  primary  form  of  documentation  in  this  chapter  is  plots  of 
various  vehicle  parameters  which  indicate  overall  performance.  To  ob¬ 
tain  these  plots,  additional  code  was  added  to  version  3.4  which  directed 
the  writing  of  vehicle  parameters  to  magnetic  disk.  Data  acquisition 
occurred  in  real-time  during  Hexapod  locomotion.  The  data  was  then 
retrieved  from  disk,  processed  if  necessary,  and  output  on  a  plotter 
interfaced  to  the  POP- 11/70  control  computer. 
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5.2  Active  Compliance 

The  compliance  servo,  shown  in  Figure  4.4,  was  si ightly  modified 
prior  to  implementation  in  Hexapod  Control  Program  version  3.4.  In  the 
event  of  force  transducer  failure,  the  unmodified  control  structure 
could  lead  to  a  position  offset  of  about  15  inches.  To  avoid  this 
potential  problem,  a  limiter  was  added  which  clips  the  force  error  at 
a  value  which  results  in  a  four  inch  position  offset.  When  the  system 
is  operating  with  normal  force  distributions,  the  force  error  is  small, 
and  the  limiter  does  not  affect  the  system  at  all.  When,  however,  a 
foot  contacts  the  ground  at  the  end  of  transfer  phase,  a  large  amplitude 
pulse  of  measured  force  can  result.  The  limiter  keeps  this  pulse  from 
exciting  the  system  unnecessarily. 

The  compliance  servo  was  implemented  with  provision  for  enabling 
active  compliance  along  each  of  the  three  body  axes,  as  described  in 
Chapter  4.  The  Hexapod  feet,  however,  exhibit  a  large  amount  of  passive 
compliance  along  the  x  and  y  axes,  due  primarily  to  the  compliance  of 
the  Hexapod  frame.  To  make  the  experimental  results  easier  to  inter¬ 
pret,  active  compliance  was  enabled  along  the  body  z-axis  only. 

The  first  on-ground  test  of  the  active  compliance  algorithm  was 
performed  with  the  Hexapod  stationary;  the  control  loop  stability  was 
tested,  as  was  the  response  to  external  force  inputs.  When  a  force  was 
applied,  the  vehicle  did  exhibit  active  compliance.  However,  the 
interaction  between  the  force  feedback  and  the  passively  compliant  frame 
of  the  vehicle  resulted  in  small  oscillations  of  the  joint  actuators  and 
of  the  vehicle  frame.  The  oscillation  frequency  was  approximately 


the  resonant  frequency  of  the  frame.  To  eliminate  these  oscillations, 
it  was  decided  to  include  a  first-order,  low-pass  filter  in  the  force 
feedback  loop.  The  filter  break  frequency  was  determined  experimentally 
by  starting  with  a  large  break  frequency  and  then  reducing  it  until  the 
oscillations  were  eliminated.  The  value  required  was  found  to  be  two 
radians  per  second.  A  block  diagram  of  the  compliance  servo  which  in¬ 
cludes  force  error  limiting  and  filtering  is  shown  in  Figure  5.1. 

Before  evaluating  the  performance  of  the  compliance  servo,  it  was 
desired  to  establish  a  base-line  servo  performance  reference  for  pur¬ 
poses  of  comparison.  The  z-axis  position  accuracy  of  a  foot  using  the 
Jacobian  servo  routine  of  Figure  2.8  supplies  such  a  reference.  Figure 
5.2  shows  the  results  obtained  while  cycling  the  legs  in  the  air  with 
only  the  leg  mass  loading  the  joint  actuators.  The  plot  shows  a  slight 
amount  of  phase  lag  between  desired  and  actual  positions,  and  also 
shows  a  steady-state  position  error  of  approximately  0.4  inches  due  to 
motor  stalling.  The  exponential  decays  following  the  half-sine  wave 
trajectories  occur  when  the  desired  position  reaches  zero,  at  which 
time  the  desired  velocity  command  is  set  to  zero,  resulting  in  a 
heavily  damped  decay. 

The  actual  system  response  can  now  be  compared  to  the  ideal 
system  response  predicted  in  Chapter  4.  To  accomplish  this,  a  computer 
modeling  routine  was  written  which  simulates  the  compliance  servo, 
modeling  the  Hexapod  as  an  ideal  system.  A  block  diagram  of  the 
modeling  routine  is  shown  in  Figure  5.3.  The  inputs  to  the  model  are 
the  inputs  to  the  real  system,  as  recorded  on  magnetic  disk.  The 
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Figure  5.3  Compliance  Servo  Modeling  Routine.  Reference  inputs  and  actual  force 
recorded  during  locomotion  tests. 
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recorded  ground  reaction  forces  are  also  used  by  the  modeling  routine, 
since  these  forces  are  inpossible  to  model  accurately.  The  solid  trace 
in  Figure  5.4  shows  a  foot  trajectory  of  the  actual  system,  obtained 
while  the  legs  were  cycling  in  the  air  with  active  compliance  enabled. 
The  dashed  curve  is  the  ideal  system  response  as  predicted  by  the 
modeling  routine.  The  ideal  trajectory  agrees  very  closely  with  the 
actual  results,  differing  only  in  the  same  respects  as  the  base-line 
reference  experiment.  When  this  experiment  was  repeated  on  the  ground, 
the  results  were  comparable. 

In  order  to  test  the  force  setpoint  generation  algorithm  indepen¬ 
dently,  the  Hexapod  was  walked  with  the  force  feedback  loop  disabled. 
The  resulting  passive  foot  forces  were  compared  with  the  output  of  the 
force  setpoint  algorithm.  Figure  5.5  shows  the  results  obtained  for 
leg  #1  while  walking  in  a  tripod  gait.  The  close  agreement  between  the 
actual  force  and  the  force  setpoint  should  be  expected;  there  are 
exactly  three  legs  on  the  ground  in  a  tripod  gait,  and  thus  equations 
4.48  to  4.50  have  a  unique  solution.  In  other  gaits,  however,  this 
system  of  equations  is  underspecified,  and  has  a  infinite  number  of 
solutions.  Thus,  the  setpoints  generated  by  the  pseudo-inverse  algor¬ 
ithm  may  not  agree  well  with  the  passive  force  distribution,  since 
small  uncertainties  in  leg  positions  and  passive  compliances  determine 
which  of  the  infinite  number  of  solutions  is  valid.  To  observe  the 
underspecified  case,  the  experiment  was  repeated  with  a  leg  duty  factor 
of  2/3,  meaning  that  four  legs  were  in  support  phase  at  all  times.  The 
results,  plotted  in  Figure  5.6,  show  the  presence  of  a  component,  of  the 
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Figure  5.6  Passive  Force  Distribution  with  Leg  Duty  Factor  of  2/3.  Plot  obtained 
on  level  terrain.  Four  legs  are  in  support  phase  at  all  times. 


homogeneous  solution.  Note  that  for  a  given  set  of  feet  in  support 
phase,  the  homogeneous  component  is  approximately  constant,  and  the 
force  tracks  well. 

5.3  Attitude  Control 

To  test  the  performance  of  the  attitude  control  algorithm  over 
irregular  terrain,  an  obstacle  was  constructed  which  was  large  enough 
to  seriously  obstruct  the  Hexapod's  locomotion  if  non-adaptive  control 
algorithms  were  used.  The  obstacle  is  constructed  of  4"  by  6"  oak 
beams,  and  is  two  beams  deep,  giving  a  maximum  height  of  8  inches.  A 
photograph  of  the  Hexapod  walking  across  the  obstacle  is  shown  in 
Figure  5.7. 

In  order  to  test  the  attitude  control  algorithm  independently  of 
the  force  control  algorithm,  the  attitude  control  tests  were  performed 
using  a  tripod  gait.  This  allows  the  force  feedback  loop  to  be  disabled, 
since  the  foot  force  distribution  is  uniquely  determined.  To  establish 
a  reference  for  comparison  for  the  performance  of  the  attitude  control 
algorithm,  the  Hexapod  was  walked  across  the  obstacle  with  no  attitude 
control.  The  resulting  vehicle  attitude  traces  are  shown  in  Figure  5.8. 
The  traces  show  a  maximum  vehicle  pitch  of  8  degrees,  along  with  large 
angular  rate  transients.  Note  that  the  periodic  nature  of  the  traces  in 
Figure  5.8  do  not  reflect  any  control  loop  instability,  but  rather 
is  caused  by  locomotion  across  obstacles  of  increasing  height  while 
using  a  periodic  gait.  Although  the  vehicle  did  not  become  statically 
unstable,  it  has  been  observed  to  do  so  at.  pitch  angles  of  less  than  15 
degrees.  Thus,  a  slightly  more  formidable  obstacle  would  prove 
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impossible  to  traverse  without  the  use  of  terrain  adaptive  algorithms. 

With  an  attitude  control  reference  established,  locomotion  using 
attitude  control  can  be  evaluated.  The  results  shown  in  Figure  5.9  were 
obtained  while  traversing  the  obstacle  with  the  attitude  control  loop 
enabled.  The  plot  shows  a  large  improvement  over  the  attitude  reference 
test.  The  maximum  angular  displacement  is  two  degrees.  The  attitude 
is  maintained  within  0.2  degrees  of  the  vertical  except  during  the 
transients  caused  by  legs  leaving  and  contacting  the  ground,  and  the 
maximum  duration  of  a  transient  is  one  second. 

5.4  Attitude  Control  with  Active  Compliance 

The  attitude  control  algorithm  was  seen  in  section  5.3  to  be  very 
effective  using  a  tripod  gait,  but  it  remains  to  be  evaluated  in  the 
case  of  gaits  with  non-unique  foot  force  solutions.  To  study  this  more 
general  case,  the  Hexapod  was  walked  across  the  obstacle  with  the  atti¬ 
tude  loop  enabled  while  using  a  leg  duty  factor  of  2/3.  The  results  of 
this  locomotion  test  with  the  force  feedback  loop  disabled  are  shown  in 
Figure  5.10.  When  the  test  was  repeated  with  the  force  feedback  loop 
enabled,  the  plot  in  Figure  5.11  was  obtained.  Comparing  the  two 
figures,  it  is  apparent  that  the  force  control  algorithm  reduced  the 
size  of  most  of  the  foot  contact  and  liftoff  transients,  thus  allowing 
the  attitude  control  loop  to  maintain  a  more  level  body  attitude. 

5.5  Force  Tracking 

With  the  attitude  control  algorithm  providing  a  stable  body  atti¬ 
tude,  as  was  assumed  in  the  development  of  the  force  control  law,  it  is 
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Figure  5.10  Vehicle  Attitude  Across  Obstacle  in  Underspecified  Gait  Using  Attitude 
Control.  Plot  was  obtained  using  a  leg  duty  factor  of  2/3. 


Vehicle  Attitude  Across  Obstacle  in  Underspeci 
and  Active  Compliance.  Plot  was  obtained  usir 


informative  to  observe  the  accuracy  with  which  the  actual  force  tracks 
the  desired  force.  From  the  last  experiment  of  section  5.4  (leg  duty 
factor  =  2/3,  attitude  and  force  control  loops  enabled),  the  force 
tracking  of  leg  #1  was  plotted,  and  is  shown  in  Figure  5.12.  In  general, 
the  actual  force  tracks  the  desired  force  quite  well.  The  ringing 
apparent  in  the  actual  force  is  due  to  underdamped  passive  compliances 
in  the  Hexapod  structure.  It  is  this  frequency  component  that  caused 
the  instability  described  in  section  5.2,  and  which  was  removed  by  the 
inclusion  of  the  low-pass  filter  in  the  force  feedback  loop. 

A  second  discrepancy  in  the  force  tracking  is  also  apparent  in 
Figure  5.12.  Note  that  the  actual  force  may  become  non-zero  before  the 
commanded  force  does  so.  This  situation  contributes  to  transients,  as 
reflected  in  the  vehicle  attitude  plots,  and  is  due  to  the  fact  that  a 
leg  is  switched  into  support  phase  as  a  function  of  foot  trajectory 
timing  and  not  as  a  function  of  ground  contact.  It  is  possible  to  use 
contact  sensing  [4]  to  minimize  this  effect,  although  vehicle  height 
must  then  be  regulated  explicitly. 

5.6  Attitude  Sensor  Evaluation 

In  Chapter  3,  two  separate  attitude  sensing  systems  were  imple¬ 
mented:  a  vertical  gyroscope  and  gravitational  pendulums.  In  the  pre¬ 
vious  sections  of  this  chapter,  all  vehicle  attitude  information  was 
obtained  from  the  vertical  gyroscope.  To  test  the  stability  of  the 
attitude  control  loop  when  using  the  pendulums  as  attitude  transducers, 
an  experiment  was  performed  with  a  zero  commanded  rate  for  the  Hexapod 
body  and  with  the  attitude  control  loop  enabled.  The  vehicle  became 


unstable  with  respect  to  the  body  roll  axis,  as  shown  in  Figure  5.13. 
This  instability  is  due  to  the  fact  that  the  pendulum  is  quite  sensitive 
to  lateral  accelerations,  which  can  be  induced  by  the  response  of  the 
attitude  control  system.  The  pendulum  output  could  be  low-pass  filtered 
to  eliminate  the  resonances,  but  this  would  preclude  the  high  bandwidth 
response  desired  of  the  attitude  control  system.  The  pendulums  are, 
however,  useful  as  physically  reliable  backup,  and  can  be  used  to  check 
for  proper  gyroscope  erection  under  static  conditions. 
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Chapter  6 

SUMMARY  AND  CONCLUSIONS 


The  problem  of  autopilot  design  for  rough-terrain  locomotion  by 
a  hexapod  vehicle  has  been  studied  in  this  thesis.  Hardware  has  been 
designed  and  control  algorithms  developed  which  allow  rough-terrain 
locomotion  with  only  directional  inputs  being  provided  by  the  operator. 
The  results  have  shown  that  if  the  necessary  sensors  are  present  on  a 
legged  vehicle,  the  problems  of  joint  coordination,  rough-terrain 
accommodation,  and  body  regulation  can  be  solved  by  a  digital  computer, 
leaving  the  vehicle  operator  free  to  perform  higher-level  control 
functions . 

6.1  Research  Contributions 

The  implementation  of  the  autopilot  function  has  required  that 
several  well-defined  problems  be  solved.  The  first  of  these  tasks  was 
the  addition  of  vector  force  sensors  to  each  leg.  The  lateral  sensor 
design  is  identical  to  the  previous  design,  but  the  vertical  sensor 
design  is  similar  only  in  the  use  of  a  piston  to  transmit  the  ground 
reaction  force.  The  sensor  performance  is  completely  satisfactory,  and 
there  have  teen  no  reliability  problems  in  the  10  months  the  sensors 
have  been  in  operation. 
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Vehicle  attitude  sensors  have  also  been  added  to  enable  automatic 
body  attitude  regulation.  Two  types  of  attitude  sensors  were  installed 
on  the  Hexapod:  gravitational  pendulums  and  a  vertical  gyroscope.  The 
vertical  gyroscope  has  proven  to  be  an  acceptable  design  solution.  It 
has  been  shown  that  pendulums  are  not  an  acceptable  solution  if  rapid 
response  is  required  of  the  attitude  control  system. 

System  reliability  has  been  improved  by  implementing  self¬ 
diagnostic  capabilities  in  the  digital  interface  between  the  vehicle 
and  its  control  computer.  By  providing  direct  computer  control  over 
vehicle  subsystems  power,  the  chance  of  human  error  has  been  reduced. 
Since  these  additions  have  been  made,  vehicle  readiness  has  been  much 
improved  and  research  has  proceeded  more  rapidly. 

To  provide  a  suspension  system  for  distributing  force  among  all 
supporting  legs  on  uneven  terrain,  linear  force  feedback  has  been  used 
to  implement  active  compliance.  The  active  compliance  algorithm  has 
been  examined  in  detail,  and  constraints  on  the  system  setpoints  have 
been  found  which  result  in  various  types  of  system  responses.  Analytic 
expressions  have  been  found  for  all  system  gains  as  functions  of  over¬ 
all  system  response.  A  closed-form  solution  has  been  found  for  the 
force  setpoint  problem  based  on  a  pseudo-inverse  force  minimization. 

To  provide  automatic  body  regulation,  a  closed  loop  attitude 
regulation  scheme  has  been  designed.  This  represents  a  level  of  con¬ 
trol  never  before  implemented,  on  tnis  vehicle,  and  is  easily  expandable 
into  a  system  which  implements  arbitrary  body  attitudes.  The  attitude 
regulation  system  has  been  shewn  to  work  well  under  all  test  conditions. 
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6.2  Research  Extensions 

Several  system  modifications  have  been  seen  to  be  desirable  as  a 
result  of  the  experiments  performed  in  this  research.  The  modifications 
suggested  below  are  direct  extensions  of  the  autopilot  system  developed 
in  this  work,  and  should  result  in  improved  performance  and  better  con¬ 
trol  flexibility. 

The  most  obvious  of  the  proposed  improvements  is  the  structural 
stiffening  of  the  OSU  Hexapod  Vehicle.  In  addition  to  making  experi¬ 
mental  results  more  difficult  to  interpret,  the  excessive  compliance 
present  in  the  vehicle  frame  contributes  to  both  dynamic  and  static 
instability.  A  stiffer  frame  should  decrease  the  amplitude  and  in¬ 
crease  the  frequency  of  structural  resonances. 

The  Hexapod  is  affected  by  transients  caused  by  foot  placement 
and  lifting  even  with  the  attitude  control  and  force  feedback  loops 
enabled.  It  is  believed  that  these  transients  could  be  greatly  re¬ 
duced  by  the  use  of  ground  contact  sensing,  where  the  foot  height  set- 
point  during  support  phase  is  determined  by  the  height  at  which  it 
made  contact  with  the  ground.  This  will  require  the  addition  of  a 
vehicle  altitude  control  loop  due  to  cumulative  errors  in  ground  height 
sensing . 

To  properly  implement  contact  sensing,  the  transfer  phase  tra¬ 
jectory  must  be  modified  to  include  a  search  phase  in  which  the  foot 
probes  downward  in  search  of  the  ground.  In  addition,  the  non-vertical 
foot  velocity  components  should  be  zero  with  respect  to  the  ground 
while  the  foot  is  below  the  maximum  obstacle  height.  The  foot 
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trajectory  should  not  allow  velocity  discontinuities  except  at  ground 
contact. 

It  should  be  possible  to  reduce  the  foot  contact  transients  in 
underspecified  gaits  by  using  a  different  algorithm  for  force  setpoint 
specification.  The  present  algorithm  minimizes  the  sum  of  squares  of 
foot  forces,  but  due  to  the  non-backdriveable  worm  gear  reductors,  it 
is  of  debatable  value  for  reducing  energy  consumption.  An  algorithm 
which  minimizes  the  discontinuities  in  commanded  force  at  foot  contact 
and  liftoff  should  reduce  the  actual  force  transients. 

The  software  needs  to  be  expanded  and  restructured  to  allow  for 
arbitrary  body  attitudes.  A  "body  servo"  routine  can  be  written  which 
accepts  attitude  and  altitude  setpoints,  compares  them  with  the  actual 
values,  and  modifies  the  foot  state  setpoints  accordingly.  The  control 
loops  can  be  closed  in  the  same  manner  as  the  existing  attitude  control 
loop.  The  attitude  and  altitude  setpoints  could  be  specified  either 
by  a  human  operator  or  by  a  higher-level  algorithm. 


108 


APPENDIX  A 
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DERIVATION  OF  THE  FOOT- FORCE  ROTATION  MATRIX 


The  geometry  of  a  right-side  (even  numbered)  hexapod  leg  is 
shown  in  Figure  A.l .  In  deriving  the  rotation  matrix  R(0),  note  that 
force  is  a  sliding  vector,  and  therefore  no  linear  position  offsets 
need  to  be  considered.  The  right  side  rotation  matrix  R2(?)  can  be 
expressed  as  the  produce  of  two  simple  rotation  matrices: 

R2($)  =  Ct1(^)][t2(91,  e2)]  (A.l) 

where  T2  rotates  vectors  from  the  foot  coordinate  system  into  the  hip 
coordinate  system,  and  T-j  rotates  vectors  from  the  hip  coordinate  sys¬ 
tem  into  the  body  coordinate  system. 

To  derive  T-|(6-|,  02),  note  that  since  linear  offsets  are  neglect 
ed,  9-|  and  62  will  appear  only  in  the  term  (9-j  +  02).  One  can  there¬ 
fore  define 

0  E  91  +  92  .  (A. 2) 

Figure  A. 2  is  a  simplified  representation  of  foot  fo.  ce  vectors,  with 
the  foot  coordinate  system  rotated  an  arbitrary  amount  fiom  the  hip 
coordinate  system.  This  figure  is  obtained  by  removing  the  linear 
offsets  from  Figure  A.l  and  then  lumping  the  rotary  displacements 
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61  and  62-  By  defining 


=  ^fxf  fyf 

as  the  actual  force  expressed  in  the  foot  coordinate  system,  and 

h  5  tfx*  fyh  f2h]T  (A.4) 

as  the  actual  force  expressed  in  the  hip  coordinate  system,  one  can 
write 

Fh  =  [T2(6)]?f  .  (A. 5) 


Referring  to  Figure  A. 2,  it  is  seen  that 


V 

cos  6  0  sin  e 

f  x  f 

fyh 

= 

0  1  0 
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-sin  e  0  cos  0 
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Then 


t2(s)  = 


cos  8 
0 

-sin  0 


0  sine 
1  0 
0  cos  8 


By  defining 


PyB  f.’B] 


(A. 6) 


(A. 7) 


;i2 


as  the  actual  force  expressed  in  the  body  coordinate  system,  one  can 
write 


(A. 8) 


Figure  A. 3  depicts  this  transformation,  and  is  obtained  in  a  manner 
analogous  to  Figure  A. 2.  From  this  figure  it  is  seen  that 


sin  ip  -cos  \p  0  f 
cos  ip  sin  (p  0  f 


1  f. 


(A. 9) 


and  therefore 


sin  ip  -cos  ip  0 

T2(tp)  =  cos  ip  sin  ip  0 


(A. 10) 


Combining  equations  A.l ,  A. 2,  A. 6,  and  A. 10,  the  final  result 


sin  ip  cos(6i  +0£)  -cos  w  sin  sinC©^  ) 

R2(6)  =  cos  :P  cos(  6-|  +6^)  sin  ip  cos  <p  s i n ( 6^  +0^  ) 


(A. 5) 


-si n ( 91 +^2 ) 


cos(91 +e2) 


is  obtained. 


Fhe  left  side  rotation  matrix  R-|(?)  is  derived  similarly,  and  is 


given  by 
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sin  tKos(6-|+e2) 
-cos  i^cos(6.|+e2) 
-sin(e1 +02) 


cos  \p  sin  \p  sin(e1+e2) 
sin  \p  -cos  ip  sin(e^+e2) 
0  cos(0^+e2) 


(A. 6) 
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APPENDIX  B 


SOLUTION  OF  THE  FOOT- FORCE 
SETPOINT  PROBLEM  FOR  A  MULTILEGGED  VEHICLE 

For  a  legged  vehicle  to  maintain  static  equilibrium,  the 
following  equations  must  be  satisfied. 


SMx  -  0 

(sum  of  moments  about  x-axis) 

(B.l) 

IM  =  0 

y 

(sum  of  moments  about  y-axis) 

(B.2) 

EFZ  =  Ftotal 

(sum  of  vertical  forces) 

(B.3) 

On  a  hexapod  vehicle,  there  will  be  from  three  to  six  legs  in  support 

phase.  Numbering  only  legs  in  support  phase,  equations  B.l ,  B.2,  and  B.3  be¬ 
come 


Vl  +  V2  + 

• • .  +  f_y_  =  0 

n  n 

(B  .4) 

flxl  +  f2x2  + 

• "  +  fnxn  3  0  3  <  n  <  6 

(B  .5) 

fl  +  f2  + 

+  Fn  ~  Ftotal 

(B.6) 

These  can  be  written  as 


>1  y2 

yn 

V 

0 

X1  x2 

...  xn 

f2 

= 

0 

_  1  1 

...  1  . 

• 

_Ftotal_ 

This  system  is  underspecified  except  for  the  special  case  n  =  3, 
in  which  case  the  system  is  exactly  specified.  Thus,  a  pseudo¬ 
inverse-type  solution  may  be  applied  to  obtain  a  minimum  sum  of 
squares  of  forces. 

Given  the  underspecified  system 

A  z  =  c  (B.8) 

it  can  be  shown  that  a  minimum  norm  solution  for  z  must  lie  in  the 
row  space  of  A.  This  condition  can  be  expressed  as 

2  =  AT  w  (B .9) 

where  w  is  a  weighting  vector  for  the  rows  of  A.  Substituting 
equation  B.9  into  B.8  gives 

A  AT  w  =  c  (B.10) 

The  matrix  A  AT  is  nonsingular  if  A  has  full  row  rank,  and  therefore 
Gaussian  elimination  may  be  employed  to  solve  for  w.  The  value  of  z 
can  then  be  obtained  from  equation  B.9. 


For  the  given  system. 


( 


t 


i 


Using  Gaussian  elimination  and  dropping  subscripts,  the  following 
augmented  matrices  are  obtained. 


(B  .1 3) 


(B.14) 


1 1  7 


l 


The  other  denominator  term  is 


2  2  2 
d  =  lx  £y  -  (Ixy)  . 

Consider  the  n  coordinates  x,  to  represent  a  vector  x  in  Rn.  Likewise, 
there  exists  a  vector  y  in  Rn  defined  by  the  n  coordinates  y, . 

Then 

lx/  =  |x|2 

iy,2  =  I y 1 2 

(lxiy,)2  =  (x*y)2  =  (|x|  | y |  cose)2 
=  j x j 2  |yj 2  cos20 

Thus 

d  =  |x|2  |y | 2  -  |x|2  |y | 2  cos20 

•*  d  >  0  ,  and  d  =  0  if  0  =  mi,  n  any  integer 

+  y  =  K  x 

-*■  y,  =  kx;  i  =  1  . . .  n 

i.e.,  the  denominator  term  is  zero  only  if  all  the  feet  are  in 
a  straight  line,  which  is  again  a  forbidden  condition. 
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S  =  n  -  — 2  I^Zy)  +(ZxZy  -ZxyZy)Rj  (B.20) 

The  augmented  matrix  from  equation  B.10  is  then 


• 

1 

0 

Q 

!  0 

0 

1 

R 

i  0 

(B.21) 

0 

0 

S 

!  F 

From  the  physical  description  of  the  problem  (given  that  all  feet  are 
not  in  a  straight  line)  it  is  obvious  that  the  system  is  solvable,  and 
therefore  that  the  matrix  AAT  is  nonsingular.  Thus  the  pivot  element 
S  must  be  nonzero,  and  final  augmented  matrix  is  obtained  as 


1 

0 

0 

-H 

m5  = 

Q 

1 

0 

Li- 

C£|t/> 

1 

0 

0 

1 

From  equation  8.25,  the  intermediate  result  w  is  seen  to  be 


The  final  solution  is  obtained  from  equations  B.9  and  B.23, 


giving 


yl  X1  1 

y2  *2  1 


-  R/S 


(B  .24) 


.  yn  ML1 


where  z  is  the  vector 


and  f  .  is  the  optimal  force  for  leg  i.  ( B . 25 ) 


Then  for  any  leg  i  in  support  phase,  the  normalized  result 


p  ~ —  =  ^  (1  -  Qyi  -  Rx.) 
total  b  1  1 


(B .26) 


is  obtained  from  equations  B.24  and  B.25. 


APPENDIX  C 


HEXAPOD  CONTROL  PROGRAM  VERSION  3.4 


<***-*) 

/*«**«*  FILE!  GBLF34.PAS  «**«**/ 
/imiuiMttmtiimtimtMittuttttutMtmmmmtmtitt/ 


/* 

*/ 

/* 

FUNCTIONS 

THIS  FILE  CONTAINS  GLOBAL  DECLARATIONS  FOR 

*/ 

/t 

ROBOT  3.4.  THE 

EXECUTABLE  FILES  WHICH  SHARE 

*/ 

/* 

THESE  GLOBALS 

ARES 

*/ 

/* 

*/ 

/* 

R0BT34.PAS 

PLAN34.PAS  F00T34 . PAS 

*/ 

/« 

INI T  34 .PAS 

LINE34.PAS  SER  34 .PAS 

*/ 

/* 

DLNK34.PAS 

LIBR34.PAS 

t/ 

/* 

*/ 

/ttttttt **t*t*t*t*tXt*t*ttt******«»Mtt St *«*«***« ********** **»*«*/ 

CONST 

PI 

*  3.14159! 

FSCALE 

=  -200.0! 

/*  FORCE  SCALE  FACTOR 

*/ 

MAXSTROKE  «=  12.0! 

/*  MAXIMUM  FOOT  STROKE 

*/ 

MOVE 

»  TRUE! 

/*  SWITCH  TO  ENABLE  SERVOING 

*/ 

NOMOVE 

a  FALSE! 

/*  SWITCH  TO  DISABLE  SERVOING 

*/ 

TYPE 

ARRAY*  - 

ARRAYC1. .63 

OF  REAL! 

ARRAY18  ■ 

ARRAYCO. • 173  OP 

REAL* 

MODETYPE  - 

(  RANDOM • 

NEUTRAL*  PREWALK* 

CRUISE* 

SIDESTEP*  TURN  >i 

VAR 

MIDSTXi  NIDSTY. 

/« 

MIDSTANCE  COORDINATES 

*/ 

RPHASE. 

/» 

RELATIVE  LEG  PHASES 

*/ 

XFA.  YFA. 

ZFA. 

/* 

ACTUAL  FOOT  FORCES 

*/ 

XFD.  YFD . 

ZFD  * 

/* 

DESIRED  FOOT  FORCES 

*/ 

XFF  >  YFF  < 

ZFF. 

/* 

FILTERED  FOOT  FORCES 

*/ 

XPA,  YP  A  * 

ZP  A  * 

/* 

ACTUAL  FOOT  POSITIONS 

%/ 

XPD i  YPD * 

ZPD. 

/* 

DESIRED  FOOT  POSITIONS 

*/ 

XRD «  YRD* 

ZRD* 

t* 

DESIRED  FOOT  RATES 

*/ 

ZPTERM 

/* 

Z  POS.  TERM  FOR  ATT.  CONTROL 

*/ 

• 

• 

ARRAY6! 

ZEROFORCE 

/* 

TRUE  FORCE  OFFSETS 

%/ 

• 

ARRAY  IS ! 

FZERO 

/* 

FLAGS  FOR  FORCE  ZEROING 

%/ 

* 

• 

ARRAYC1 . .63 

OF  BOOLEAN! 

I  cl 


4 


l 


ATTITUDE* 

/t 

SWITCH 

FOR 

ATTITUDE  CONTROL 

*/ 

COMPLIANCE* 

/* 

INDIRECT  COMPLIANCE  SWITCH 

*/ 

OPTIMIZATION* 

/* 

SWITCH 

FOR 

OPTIMAL  FORCE 

*/ 

PASS1 , 

/* 

SWITCH 

TO 

INITIALIZE  FILTER 

*/ 

SAVE, 

/* 

SWITCH 

FOR 

DATA  AQUISITION 

%/ 

SPRING 

/* 

SWITCH 

FOR 

ACTIVE  COMPLIANCE 

*/ 

5  BOOLEAN* 


COMMAND 


/*  OPERATOR  INPUT  COMMAND 


*/ 


I 


:  CHARI 


LASTCLOCK* 

TOTALCLOCK 


/*  STORAGE  FOR  CLOCK  BUFFER 
/*  CLOCK  TICK  ACCUMULATOR 


%/ 

*/ 


:  INTEGERI 


MODE 


\ 


MODETYPEI 


BETA. 

DPSI* 

DT. 

FOOTLIFT. 

MIDSTZ. 

NUELX  *  NVELY  * 
NDPSI. 

PERIOD, 

PHASE* 

RADIUS, 

SPERIOD* 

VELX,  VELY, 
VELMAX 


SUPPORT 


REAL! 


SET  OF  I. .61 


/*  HEXAPOD  OPERATING  MODE  */ 


/*  LEG  DUTY  FACTOR  */ 
/*  FILTERED  TURN  RATE  COMMAND  */ 
/*  DELTA  TIME  (SEC)  */ 
/*  FOOT  LIFTING  HEIGHT  »/ 
/*  MIDSTANCE  Z  COORDINATE  */ 
/*  OPERATOR  VELOCITY  COMMANDS  */ 
/*  OPERATOR  TURN  RATE  COMMAND  */ 
/»  PERIOD  OF  KINEMATIC  CYCLE  %/ 
/*  KINEMATIC  CYCLE  PHASE  */ 
/*  RADIUS  FROM  CO.  TO  MIDSTANCE  */ 
/*  SUPPORT  PERIOD  (SEC)  %/ 
/*  FILTERED  VELOCITY  COMMANDS  */ 
/*  MAX  FOOT  VELOCITY  COMPONENT  */ 


/*  SET  OF  LEGS  IN  SUPPORT  PHASE  %/ 
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« 


/**«**«  FILES  R0BT34.PAS  ******/ 


/I*****************************/ 

/*  program:  robot  3.4  %/ 

/«*****************«***********/ 

/ttmtMtmtutumnimtmttutmttttttttttmtmtctmt/ 


/* 

programmers: 

TED  CHANG »  DENNIS  PUGH 

*/ 

/* 

date: 

30-MAR-82 

*/ 

/* 

»/ 

/* 

PROGRAMMER 

*/ 

/* 

guide: 

>PAS  ROBT34-GBLF34, 

ROBT34 

*/ 

/* 

>MAC  R0BT34=R0BT34 

*/ 

/* 

>TKB  BR0BT34 

*/ 

/* 

*/ 

/* 

link  files: 

LIBR34 

*/ 

/* 

PLAN34 

*/ 

/* 

F00T34 

*/ 

/* 

SERV34 

*/ 

/* 

DLNK34 

*/ 

/* 

INIT34 

*/ 

/* 

LINE34 

*/ 

/* 

KYCK34 

*/ 

/* 

P0UR34 

*/ 

/* 

*/ 

/* 

user  guide: 

>RUN  ROBT34 

*/ 

/* 

THE  PROGRAM  PROMPTS 

FOR 

INSTRUCTIONS 

*/ 

/* 

»/ 

/* 

(THIS  PROGRAM  IS  TO 

RUN 

ON  THE  PDP  11/70) 

*/ 

/* 

*/ 

/*******■******************************»**************************/ 


CONST 

ZRMAX 

*  6.0)  /*  MAXIMUM  FOOT  Z  -  VELOCITY 

*/ 

TYPE 

CAPITALS 

*  'A' . . 'Z'  ) 

CHARSET 

«  SET  OF  CAPITALS! 

SETARRAY 

*  ARRAYC  MODETYPE  3  OF  CHARSET) 

VAR  DPSIMAX 

:real; 

/* 

MAXIMUM  TURN  RATE 

*/ 

VTMAX 

:real) 

/* 

MAX.  FOOT  VELOCITY  COMPONENT 

*/ 

BETAMODE 

‘.INTEGER! 

/% 

LEG  DUTY  FACTOR  INDEX 

%/ 

LETTER 

!  CHAR ) 

/* 

LOOP  INDEX 

*/ 

HALTSET 

! CHARSET ) 

/% 

SET  OF  COMMANDS  FOR  UHICH 

*/ 

/% 

HEXAPOD  HALTS  AFTERUARD 

%/ 

NEXTCOM 

(SETARRAY! 

/% 

SET  OF  VALID  COMMANDS  FOR 

*/ 

/% 

FOR  EACH  MODE 

*/ 

PROCEDURE 

NORMALIZE!  EXTERNAL! 

PROCEDURE 

INITIALIZE!  EXTERNAL! 

PROCEDURE 

HALT! 

EXTERNAL! 

PROCEDURE 

UPD0UN1 

HEIGHT!  REAL  >!  EXTERNAL! 

PROCEDURE 

PLANMOTION !  EXTERNAL! 

PROCEDURE 

CLOCKINIT!  EXTERNAL! 

PROCEDURE 

KEYINT  1 

FORTRAN! 

PROCEDURE 

KEYCKtUAR  COMMAND!  CHAR)!  FORTRAN 

PROCEDURE 

KYUAITi 

FORTRAN ! 

PROCEDURE 

IOKILL! 

FORTRAN! 

PROCEDURE 

INOUE! 

FORTRAN! 

PROCEDURE 

TESTONi 

EXTERNAL! 

PROCEDURE 

TURNON! 

EXTERNAL! 

PROCEDURE 

TURNOF! 

EXTERNAL! 

FUNCTION 

DELTATIME!  REAL!  EXTERNAL! 

BEGIN  /* 

START  MAIN  PROGRAM  */ 

/t««*t****tt**t*ttt ***********/ 
/*  PARAMETER  INITIALIZATION  */ 
/«*t***t«*tt*t ****** **********/ 


FOOTLIFT  !  =  5.0! 

BETAhODE  :=  A! 

/***  ASSIGN  MIDSTANCE  POSITION  ***/ 


MIDSTXCII 

!=  22.75 

t  1.436! 

MIDSTXC23 

:  S 

22 . 75 

+  1.436 

MIDSTXC33 

i  = 

1 .43b! 

MIDSTXC43 

:  = 

1  .  436 

MIDSTXC53 

!=  -22.75 

+  1.436! 

MIDSTXC61 

!  - 

-22.75 

t  1.436 

MIDSTYCl] 

! *  -24.0! 

MIHSTYC23 

:  = 

24.0! 

MIDSTYC3I 

!=  -24.0! 

MIDSTYC43 

J  a 

24.0! 

MIDSTYC53 

i=  -24.0! 

MIDSTYC63 

:  = 

24.0! 

MIDSTZ  5=  17.0! 

RADIUS  ! *  SORT (  MIDSTXC1I  *  MIDSTXC13  +  MIDSTYC11  *  MIDSTYC13  ) 


/*»* 


NEXTCOMMAND  ‘ ECLARATIQN 


***/ 


NEXTCOMC  •ANDQM  3 
NEXTCOMC  NEUTRAL  3 
NEXTCOMC  PREUALK  3 
NEXTCOMC  CRUISE  3 
NEXTCOMC  TURN  3 
NEXTCOMC  SIDESTEP  3 


C,I'»'M'»,N'*'X'3* 

C*D'»'I'.'M'»'N'»'U'f'X'3» 

C'C't'H't'I'f'M'j'N't'.'i'X'i'Z'Sr 

C'B'f'F'f'H'f'L'p'P'f'R'f'S'r'X'31 

C'M'*'P'»'S'»'X'3> 

C'H'»'L'<'R'»'X'3> 


HALTSET 


:»  C'M'.'N'.'I','H'»'U'.'D'»'C 


f  'T  •  'Z'  •  'W'3» 


WRITE (  CHR < 27 ) »  'C2J')i  /*  ERASE  SCREEN  */ 

WRITE  <  CHR  ( 27 )  >  'ClilHMt  /*  CURSOR  TO  HOME  */ 

TESTONJ  /*  POWER  UP  THE  HEXAPOD  */ 

TURNOFS  /*  SWITCH  OFF  MOTOR  POWER  UNTIL  NEEDED  */ 


/**********************/ 

/*  DISPLAY  MENU  */ 
/«*********************/ 

WRITE (  CHR ( 27 ) »  'C2J')I  /*  ERASE  SCREEN  */ 

WRITEt  CHR  ( 27 )  *  'CHlH'li  /t  CURSOR  TO  HOME  %/ 


URITELN< 'ENTER 

N 

TO 

NORMALIZE  THE  LEG  POSITIONS  1 

BETAMODE  = 

'  )  » 

URITELN< ' 

U 

TO 

MOVE  THE  BODY  UP  1 

'  )  ! 

WRITELN< ' 

D 

TO 

MOVE  THE  BODY  DOWN  1 

FOOTLIFT  = 

'  )  t 

URITF.LN<  ' 

I 

TO 

INITIALIZE  THE  LEG  POSITIONS 

'  )  » 

WRITELN< ' 

M 

TO 

MODIFY  THE  PARAMETERS  1 

'  )  i 

WRITELN< ' 

X 

TO 

EXIT  THE  PROGRAM  EXECUTION  1 

COMPLIANCE 

'  )  f 

WRITELN<  ' 

1 

'  )  > 

URITELN< ' 

C 

TO 

ENTER  CRUISE  MODE  1 

OPTIMIZATION 

'  )  i 

URITELN<  ' 

T 

TO 

ENTER  TURN-IN-PLACE  MODE  1 

'  >  i 

WR I TELN ( ' 

Z 

TO 

ENTER  SIDESTEP  MODE 

ATTITUDE 

'  )  ! 

URITELN<  ' 

1 

'  )  r 

WRI TELN (  ' 

F 

TO 

INCREASE  FORWARD  VELOCITY  COMPONENT  1 

'  )  i 

WRI TELN (  ' 

B 

TO 

INCREASE  REARWARD  VELOCITY  COMPONENT  1 

AQUISITION 

'  )  i 

URITELN< ' 

S 

TO 

INCREASE  CLOCKWISE  TURN  RATE 

TIME 

'  )  i 

URITELN(  ' 

P 

TO 

INCREASE  COUNTERCLOCKWISE  TURN  RATE 

'  >  > 

URITELN<  ' 

R 

TO 

INCREASE  RIGHTWARD  VELOCITY  COMPONENT 

'  )  i 

WRI TELN( ' 

L 

TO 

INCREASE  LEFTWARD  VELOCITY  COMPONENT  1 

'  )  i 

URITEt.N<  ' 

H 

TO 

HALT  MOTION 

’  >  S 

WRITELNS 
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/«**  SET  TERMINAL  CHARACTERISTICS  ***/ 

WRITE (  CHR !  27 )  »  'C?41'  >;  /*  SET  TO  NOSCROLL  */ 

URITE<  CHR< 27 ) *  'C20;22r'  )i  /*  SET  SCROLLING  REGION  */ 

WRITE (  CHR  <  27 ) »  'C?6I'  )l  /*  SET  ABSOLUTE  ORIGIN  MODE  */ 

WRI TE (  CHR  <  27  > .  'C20ilf'  )i  /*  CURSOR  TO  SCROLLING  REGION  */ 

KEYINT?  <  PLACE  KEYBOARD  INPUT  REQUEST  ON  QUEUE  > 

CLOCKINIT i  <  START  PROGRAMMABLE  CLOCK  > 


/******************************/ 

/*  REAL  -  TIME  OPERATION  */ 

/t******» *************** *******/ 

COMMAND  J=  'M')  <  ENTER  MODIFY  SEQUENCE  INITIALLY  > 

mode  :»  random; 

REPEAT  <  UNTIL  COMMAND  *  'X'  > 

WRITE (  CHR ( 27 ) t  'C22.1H'  )»  <  CURSOR  TO  SCREEN  BOTTOM  > 

WRITE<  CHR  <  27 ) »  'CO«'  )i  <  NORMAL  CHARACTER  ATTRIBUTES  > 

WRITELN. 

WRI TELN (  COMMAND  >« 

IF  NOT  <  COMMAND  IN  NEXTCOMC  MODE  3  > 

THEN  <  COMMAND  IS  INVALID  > 

BEGIN 

URITELNI  CHR ( 13  )  *  CHR < 27 ) , ' *6***  INVALID  COMMAND  ***')! 
WRITE!  'VALID  COMMANDS  ARE:  '  >> 

FOR  LETTER  : -  'A'  TO  'Z'  DO 

IF  LETTER  IN  NEXTCOMC  MODE  3  THEN  WRITE!'  '.LETTER) 
WRITELN) 

IF  MODE  IN  C  RANDOM.  NEUTRAL.  PREUALK  3 
THEN  COMMAND  :=  'W'  <  UAIT  > 

ELSE  COMMAND  :=  ' G ' !  <  GO  > 

end; 

<  END  IF  } 
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IF  COMMAND  IN  HALTSET 
THEN 

BEGIN 

CASE  COMMAND  OF 

‘ N' S  BEGIN 

NORMALIZE) 

MODE  !»  NEUTRAL) 

END) 

'I'!  BEGIN 

NORMALIZES 

INITIALIZE) 

MODE  {»  FREUALK) 

ENDS 

'U'i  UPDOUN (  24.5  )) 

'  D '  S  UPDOUNI  MIDSTZ  >) 

'H'i  BEGIN 

HALT) 

MODE  !=  FREUALK) 

SAVE  *.»  FALSE)  <  SWITCH  OFF  DATA  AOUISITION  > 

END) 

'C'S  BEGIN 

TURNON) 

URITELNS 

URI TELN (  CHR  <  27  >  r  '*6***  CRUISE  MODE  ***'>> 

MODE  is  CRUISE) 

SPRING  S *  COMPLIANCE)  <  ENABLE  ACTIVE  COMPLIANCE  > 
PASS!  S»  TRUE#  <  FLAG  FOR  FILTER  INITIALIZATION  > 
END) 

' T ' S  BEGIN 

TURNON) 

URITELN) 

WRITELNI  CHR ( 27  )  »  '*6***  TURN- 1 N-PLACE  MODE  ***')) 
MODE  i=  TURN) 

SPRING  i=<  COMPLIANCE#  <  ENABLE  ACTIVE  COMPLIANCE  > 
PASS1  S»  TRUE)  I  FLAG  FOR  FILTER  INITIALIZATION  > 
END) 

'Z'i  BEGIN 

TURNON) 

URITELN) 

WRITELNI  CHR(27)t  SIDESTEP  MODE  ***')) 

MODE  J=  SIDESTEP) 

SPRING  t=  COMPLIANCE)  <  ENABLE  ACTIVE  COMPLIANCE  > 
PASS  1  i=  TRUE)  i  FLAG  FOR  FILTER  INITIALIZATION  > 
END) 


'M'S  BEGIN 

IOKILLi  <  CANCEL  INPUT  REQUEST  > 

URITELN) 

URI TELN (  CHRS27) »  '#4PARAMETER  MODIFICATION ' ) ) 

URITELNt 

<***>  URI TELN (  'CHANGE  BETA?')) 

READLN<  LETTER  >) 

IF  LETTER  =  'Y'  THEN 
REPEAT 

URITELN< 'PLEASE  ENTER  DESIRED  BETAMODE !  ' ) ) 

URITELN( '  IS  BETA  =  5/6  2S  BETA  =  3/4')) 

URITELNS  '  3$  BETA  =  2/3  4S  BETA  «  1/2' >) 

READLNS BETAMODE) t 
UNTIL  BETAMODE  IN  Cl*2»3»43) 

WRITES  CHR ( 27) f ' 7 ' *  CHR S 27 > » ' C 1 ) 67H ' .  CHR ( 27 ) r ' C 1»'  )) 
WRITES  BETAMODE  S  1 1  CHRS27)i  '8'  >) 

URITELN) 

URITELNt 

CASE  BETAMODE  OF 


IS 

BETA 

J  3 

0.8333) 

< 

5/6 

> 

2 : 

BETA 

:  = 

0.75) 

< 

3/4 

> 

3S 

BETA 

{  s 

0.6667) 

{ 

2/3 

> 

4  S 

BETA 

:  = 

0.5 

{ 

1/2 

> 

END)/*  CASE  */ 

/***  ASSIGN  RELATIVE  LEG  PHASES  ***/ 

RPHASEC 1 3  S=  O.O) 

RPHASEC23  S=  0.5) 

RPHASEC33  !=  BETA) 

RPHASEC43  S=  BETA  -  0.5) 

RPHASEC53  S  =  2.0  *  BETA  -  1.0) 

RPHASEC43  ! ■  RPHASEC53  +  0.5) 

RPHASEC63  S=  RPHASEC63  -  TRUNC  S RPHASEC 6 3 ) ) 

<**«>  URITELNS  'CHANGE  FOOTLIFT? '  >j 
READLNS  LETTER  >) 

IF  LETTER  *  'Y'  THEN 
REPEAT 

URITELNS  'ENTER  NEW  VALUE  OF  FOOTLIFT'  )) 

READLNS  FOOTLIFT  >) 

UNTIL  S  FOOTLIFT  >=  1,0  )  AND  S  FOOTLIFT  <=  12.0  )) 
WRITES  CHR  <27)»'7'»  CHR S 27 ) » ' C 3  )  67H  '  »  CHR S 27 ) » ' C lm '  )) 
WRITES  F00TLIFTS4S1,  CHR  <  27 ) »  '8'  )( 

URITELN) 

URITELN) 

VTMAX  :=  ZRMAX  *  MAXSTROKE  /  S  FOOTLIFT  *  PI  )) 

IF  VTMAX  >  4.0  THEN  VTMAX  S=  4.0)  {  IN.  PER  SEC  > 
VELMAX  5=  VTMAX  *  <  1.0-  BETA  >  /  BETA) 

DPSIMAX  S  =  VELMAX  /  RADIUS) 


{***>  WRITELN(  'TURN  ACTIVE  COMPLIANCE  ON?')! 
REAOLN (  LETTER  >i 

WRITE!  CHR  ( 27 )  (  '7',  CHR!27>.  '  C6.56H'  ) 
IF  LETTER  »  ' T ' 

THEN 

BEGIN 

COMPLIANCE  5 ■  TRUE. 

WRITE!  CHR  (  27 )  .  'C755.li*'  >! 

END 

ELSE 

BEGIN 

COMPLIANCE  I-  FALSE. 

WRITE!  CHR ! 27  >  .  'CO*'  >5 
END. 

WRITE!  'COMPLIANCE'.  CHR!27).  '8'  >5 
WRITELN! 

URITELN5 

<***>  WRITELN!  'TURN  FORCE  OPTIMIZATION  ON? '  )  5 
READLN!  LETTER  )! 

WRITE!  CHR !  27 )  »  '7'.  CHR127).  'CB556H'  > 
IF  LETTER  =  'Y' 

THEN 

BEGIN 

OPTIMIZATION  !=>  TRUE! 

WRITE!  CHR < 27 > .  ' C 7 » S F 1 » '  >5 
END 

ELSE 

BEGIN 

OPTIMIZATION  1=  FALSE! 

WRITE!  CHR  <  27 ) »  'CO»'  X 
END! 

WRITE!  'OPTIMIZATION'.  CHR!27),  'B'  )! 
WRITELN! 

WRITELN! 

<***>  WRITELN!  'TURN  ATTITUDE  CONTROL  ON?')! 
READLN!  LETTER  )! 

WRITE!  CHR! 27) »  '7'.  CHR127).  'C10!56H' 
IF  LETTER  =  'Y' 

THEN 

BEGIN 

ATTITUDE  :=  TRUE! 

WRITE!  CHR ! 27 )  .  ' C 7 ! 5 # 1 » '  )! 

END 

ELSE 

BEGIN 

ATTITUDE  !=  FALSE! 

WRITE!  CHR ! 27 ) .  'COn'  )! 

END! 

WRITE!  'ATTITUDE'.  CHRI27).  '8'  )! 
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INQUE )  <  REASSERT  INPUT  REQUEST  > 

NODE  i>  random; 

URITELN) 

URITELNI 

URITELNI  CHR  <  27  )  »  ' 96INITI ALIZE  HEXAPOD  ')) 
ENOI 


'H'  ! 

END!  <  CASE  COMMAND  > 

KYUAITi 

KEYCKI COMMAND) I 

DT  i=  DELTATIME)  <  UPDATE  LASTCLOCK  AFTER  WAIT  > 
END  <  THEN  > 

ELSE 

BEGIN 

CASE  COMMAND  OF 


'F'  5 

IF 

NVELX 

<  <  0.9  * 

VELMAX  > 

THEN 

NVELX 

NVELX  +  ( 

0.1 

* 

VELMAX  ) 

ELSE 

NVELX  :» 

VELMAX) 

'B'  5 

IF 

NVJLX 

>  -<  0.9 

*  VELMAX 

) 

THEN 

NVELX 

NVELX  -  ( 

0.1 

* 

VELMAX  ) 

ELSE 

NVELX  :> 

-VELMAX) 

'S'  S 

IF 

NDPSI 

A 

O 

'C 

4* 

DPSIMAX 

> 

THEN 

NDPSI  :« 

NDPSI  +  ( 

0.1 

* 

DPSIMAX  ) 

ELSE 

NDPSI  1- 

DPSIMAX) 

'P' : 

IF 

NDPSI 

V 

1 

o 

>0 

*  DPSIMAX 

) 

THEN 

NDPSI  :» 

NDPSI  -  ( 

0.1 

* 

DPSIMAX  > 

ELSE 

NDPSI  :» 

-DPSIMAX) 

'R' : 

IF 

NVELY 

D- 

O 

V 

VELMAX  ) 

THEN 

NVELY  }= 

NVELY  F  ( 

0.1 

* 

VELMAX  ) 

ELSE 

NVELY 

VELMAX) 

'L'  ! 

IF 

NVELY 

>  -<  0.9 

*  VELMAX 

) 

THEN 

NVELY  !* 

NVELY  -  ( 

0.1 

* 

VELMAX  ) 

ELSE 

NVELY  :* 

-VELMAX) 

'G' : 


END)  <  CASE  COMMAND  > 

PLANMOTION)  {  MOTION  PLANNING  t  EXECUTION  > 
<  UNTIL  NEXT  COMMAND  INPUT  > 


END)  <  ELSE  > 

<  END  IF  COMMAND  IN  HALTSET  > 
UNTIL  COMMAND  =  'X'j 


IOKILLi  <  CANCEL  KEYBOARD  INPUT  REQUEST  > 


URI TE (  CHR  <  27 ) t  'C?4h')J 


URITE(  CHR < 27 ) >  'Cl»22r')» 
WRITE(  CHR  <  27  >  *  'C2J'>» 
WRITE  <  CHR ( 27 >  >  ' C 1  * 1H ' >  * 
END. 


/*  SET  TO  SMOOTH  SCROLL  */ 

/%  RESTORE  SCROLLING  REGION  */ 
/*  ERASE  SCREEN  */ 

/*  CURSOR  TO  HOME  */ 


<**£+*) 


/*«*«***  FILE!  PLAN35 . PAS  *******/ 


/*****************************/ 

/*  procedure:  planmotion  */ 
/**************«**************/ 

/*************************«**************************************/ 


/* 

*/ 

/* 

programmers:  ted 

CHANG.  DENNIS  PUGH 

*/ 

/* 

date:  30-MAR-82 

*/ 

/* 

function:  plan 

BODY  MOTION 

TO  IMPLEMENT  OPERATOR 

COMMANDS  */ 

/* 

USER  guide:  the 

CALLING  FORMAT  IS! 

*/ 

/* 

PLANMOTIONi 

*/ 

/* 

*/ 

/* 

PROCEDURES  CALLED 

!  FOOTLINE 

.  KEYCK 

*/ 

/% 

*/ 

/% 

GLOBAL  VARIABLES! 

*/ 

/* 

referenced: 

NVELX . 

NVELY. 

NDPSI 

*/ 

/* 

BETA. 

VELMAX 

*/ 

/* 

MODE 

*/ 

/* 

*/ 

/* 

modified: 

VELX  > 

VELY. 

DPSI 

*/ 

/* 

PERIOD. 

SPERIOD. 

PHASE 

*/ 

/* 

DT 

*/ 

/* 

*/ 

/*******»*******************************************************/ 

PROCEDURE  PLANMOTION! 


CONST  TIMECONST  «  0.5! 

MINSTROKE  =»  5.0! 
RhIN  »  60.0! 


VAR  DVELX  .  DVELY.  DDPSI. 
NVEL. 

LTIME. 

FOOTRATE. 

STROKE 


PROCEDURE  FOOTPATH!  EXTERNAL! 

PROCEDURE  KEYCK  <  VAR  COMMAND!  CHAR  )!  FORTRAN! 
FUNCTION  S IGN (  X!  REAL  )!  REAL!  EXTERNAL! 
FUNCTION  DELTATIME:  REAL!  EXTERNAL! 


/*  TIME  CONST.  FOR  INPUT  FILTER  */ 
/*  MINIMUM  FOOT  STROKE  */ 
/*  MIN.  TURN  RADIUS  (  CRUISE  )  */ 


/*  VELOCITY  DERIVATIVE  TERMS  */ 
/*  VELOCITY  COMMAND  MAGNITUDE  */ 
/*  TEMPORARY  STORAGE  FOR  TIME  */ 
/*  APPROX.  FOOT  RATE  URT  BODY  «/ 
/*  FOOT  TRAVEL  IN  SUPPORT  PHASE  */ 

! REAL ! 
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BEGIN  <  PLANMOTION  > 


IF  NODE  »  CRUISE  <  SET  LIMITS  ON  VELOCITY  COMPONENTS  > 

THEN 

BEGIN 

WRITELN<  CHR ( 27 ) »  'CO»'  >8  <  NORMAL  CHARACTER  ATTRIBUTES  > 

/%  SET  LIMIT  FOR  CRAB  ANGLE  IN  CRUISE  MODE  */ 

IF  ABS <  NVELY  )  >  ABS(  NVELX  ) 

THEN  NVELY  }=*  SIGN!  NVELY  )  *  ABS(  NVELX  )8 

/*  DETERMINE  MAGNITUDE  OF  COMMANDED  VELOCITY  */ 

NVEL  5=  NVELX YNVELX  +  NVELY*NVELY8 
IF  NVEL  <>  0.0  THEN  NVEL  S=*  SORT  (  NVEL  >8 

/*  LIMIT  VELOCITY  MAGNITUDE  TO  VEI.MAX  */ 

IF  NVEL  >  VELMAX  THEN 
BEGIN 

NVELX  NVELX  /  NVEL  *  VELMAX  8 
NVELY  8  =  NVELY  /  NVEL  *  VELMAX  8 
END8 

/*  SET  LIMIT  FOR  DPSI  IN  CRUISE  MODE  */ 

IF  ABS (  NDPSI  )  >  ABS (  NVEL  /  RMIN  ) 

THEN  NDPSI  SIGN (  NDPSI  )  *  ABS<  NVEL  /  RMIN  )8 

UR  I TE (  NVELX  8  8  !  3  >  NVELYJ8:3»  NDPSI!8!3  >8 
IF  ABS  <  NDPSI )  >  0.00001 

THEN  WRITE <  NVEL  /  ABSC  NDPSI  ) 58:2) 8 

END  <  THEN  > 


ELSE 

URITE<  NVELX : 8  i  3 »  NVFLY!8!3»  NDPSI!8!3  >8 
<  END  IF  MODE  > 

URITE<  CHR ( 27 )  »  'CIA'  >8  ■£  ENABLE  BOLD  CHARACTERS  > 

REPEAT  <  UNTIL  A  COMMAND  IS  INPUT  > 

/*«*  GENERATE  DT  ***/ 

DT  !=  DELTATIHEi 


/***  FILTER  THE  RATE  COMMAND  INPUTS  ***/ 


DVELX 

:=  (NVELX 

-  VELX)  / 

TIMECONST  8 

/* 

LONGITUDINAL  ACCEL. 

*/ 

DVELY 

(NVELY 

-  VELY>  / 

TIMECONST  8 

/* 

LATERAL  ACCELERATION 

*/ 

DDPSI 

:*  (NDPSI 

-  DPSI)  / 

TIMECONST! 

/* 

TURNING  ACCELA7 ION 

*/ 

VELX  : 

«  DVELXtDT 

+  VELX  8 

/* 

LONGITUDINAL  VEL. 

*/ 

VEL  *  ! 

*  DVELY*DT 

♦  VELY8 

/* 

LATERAL  VELOCITY 

%/ 

dpsi  : 

«  DDPSI *DT 

+  DPSI 8 

/* 

TURNING  RATE 

*/ 
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/***  CALCULATE  STROKE  &  SUPPORT  PERIOD  ***/ 


U’ 


CASE  MODE  OF 

cruise: 

FOOTRATE 

•  — 
t  — 

VELXS 

sidestep: 

FOOTRATE 

VELYS 

turn: 

FOOTRATE 

i  = 

RADIUS  *  DPSIS 

ENDS  <  CASE 

> 

IP  ABS <  POOTRATE  >  >  O.OOOOl 
THEN 

BEGIN 

STROKE  J*  HINSTROKE  +  (  HAXSTROKE  -  MINSTROKE  ) 

*  ABS (  FOOTRATE  )  /  VELMAXS 

SPERIOD  !=  STROKE  /  POOTRATE.  <  SUPPORT  PERIOD  > 

END 

ELSE 

SPERIOD  :  =  10000.0$ 

PERIOD  l-  SPERIOD  /  BETAS  <  TOTAL  CYCLE  PERIOD  > 


/***  UPDATE  PHASE  VARIABLE  ***/ 

PHASE  :*  PHASE  +  DT  /  PERIOD  +  IS 
PHASE  :»  PHASE  -  TRUNC < PHASE ) 5 

/***  CALL  FOOT  TRAJECTORY  GENERATION  ROUTINE  ***/ 
FOOTPATHS 

/***  CHECK  FOR  OPERATOR  INPUT  ***/ 

KEYCK(COhMAND)  S 

UNTIL  ORD<  COMMAND  >  <>  OS  <  UNTIL  A  COMMAND  IS  INPUT  > 
ENDS 
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<*•£+*) 


/**«**«*  FILE!  F00T34.PAS  *******/ 


/****«« t*t******«t ************/ 

/*  procedure:  footpath  */ 

/*******#*********************/ 

/«**************************»*************************»**********/ 


/* 

PROGRAMMERS 

:  DENNIS 

PUGH.  TED  CHANG 

*/ 

/* 

date: 

30-MAR- 

82 

*/ 

/* 

*/ 

/% 

function: 

CALCULATES  FOOT 

TRAJECTORIES  TO 

IMPLEMENT 

*/ 

/* 

THE  BODY  RATES 

COMMANDED  BY  BODY 

MOTION 

*/ 

/* 

PLANNING. 

*/ 

/* 

*/ 

/* 

user  guide: 

THE  CALLING  FORMAT  IS:  FOOTPATH} 

*/ 

/* 

*/ 

/* 

*/ 

/* 

PROCEDURES 

called: 

JSERVQ. 

RBADC 

*/ 

/* 

*/ 

/* 

GLOBAL  VARIABLES 

*/ 

/* 

referenced: 

VELX  i 

VELYt  DPSI 

*/ 

/* 

MIDSTXi 

MIDSTY  .  MIDSTZ 

*/ 

/* 

DT. 

PERIOD.  SPERIOD 

*/ 

/* 

PHASE > 

RPHASE .  BETA 

*/ 

/* 

FOOTLIFT 

*/ 

/* 

*/ 

/* 

modified: 

XPD. 

YPD.  ZPD 

*/ 

/* 

XRD. 

YRD.  ZRD 

*/ 

/* 

XFD. 

YFD.  ZFD 

*/ 

/* 

SUPPORT 

.  ZPTERM 

*/ 

/*  */ 
/M*M*******»********t********  ***************  t*****************/ 

PROCEDURE  FOOTPATH} 

CONST  ATTSCALE  *  -0.7354}  /%  ATTITUDE  SCALE  FACTOR  */ 

ATTPOLE  =  8.0}  /*  POLE  FOR  ATTITUDE  CONTROL  */ 

FTOTAL  =  285.0}  /*  TOTAL  VEHICLE  HEIGHT  (  LBS.)  */ 

PURSCALE  ■  -33333.3}  /*  POWER  SCALE  FACTOR  */ 


VAR 


ALPHA.  /*  CRAB  ANGLE  URT  LONGITUDINAL  AXIS  */ 
CGX.  CGY.  /*  COORDINATES  OF  CG.  PROJECTION  t/ 
DTI.  /*  BACKED  UP  TIME  FROM  MIDSTANCE  */ 
DXB.  DYB.  /*  BODY  DISPLACEMENTS  IN  BODY  COORD.  */ 
DXBi .  DYB1.  /*  TOUCHDOWN  DISPL.  FROM  MIDSTANCE  */ 
DXE.  DYE.  /*  BODY  DISPLACEMENT  IN  EARTH  COORD.  */ 
LPHASE.  /*  LEG  PHASE  VARIABLE  */ 
PITCH.  ROLL.  /*  ACTUAL  ANGLES  FROM  GYRO  */ 
PPITCH.  PROLL.  /*  ACTUAL  ANGLES  FROM  PENDULUM  */ 
POWER.  /*  HEXAPOD  INSTANTANEOUS  POWER  CONSUMP.  */ 
PSIC.  PSIC1.  /*  BODY  ANGULAR  DISPLACEMENT  OVER  DT  */ 
Q.  R.  S.  /*  INTERNED.  FORCE  OPTIMIZATION  TERMS  */ 
SUMX .  SUMY.  /*  SUM  OF  FOOT  DISPLACEMENTS  */ 
SUHX2 .  SUMY2.  /*  SUM  OF  SQUARES  OF  FOOT  DISPLACEMENTS  */ 
SUMXY .  /*  SUM  OF  FOOT  DISPL.  CROSS-PRODUCTS  */ 
TIMEFP.  /*  REMAINING  TIME  IN  TRANSFER  PHASE  */ 
TPHASE .  /*  TRANSFER  PHASE  VARIABLE  */ 
TRTIME.  /*  TOT-*  TIME  IN  TRANSFER  PHASE  */ 
VEL.  /*  MAC  iTUDE  OF  VELOCITY  VECTOR  */ 
ZOFF.  /*  Z  POSITION  ERROR  FOR  ATT.  CONTROL  */ 
ZRTERM  /*  Z  RATE  OFFSET  FOR  ATTITUDE  CONTROL  */ 


t  REAL) 


FRSTCH. 

I. 

N 


/*  FIRST  A/D  CHANNEL  FOR  DATA  FETCH 
/*  LOOP  COUNTER 

/*  SUPPORT  PHASE  LEG  SET  COUNTER 


i  INTEGER) 


XFTHLD.  YFTHLD  /*  DESIRED  FOOT  TOUCHDOWN  POINT 


*/ 

*/ 

*/ 


*/ 


:  ARRAY6I 


TOPBLOCK 


/*  TOP  A/D  CHANNELS 


*/ 


:  ARRAY  1 8 ) 


PROCEDURE  JSERVO (  MOVE:  BOOLEAN  >)  EXTERNAL) 

PROCEDURE  RBADC  <  NOCHAN,  FRSTCH!  INTEGER)  SCALE!  REAL)  VAR  INDATA ! 
EXTERNAL) 


ARRAY  1 8  ) 


FUNCTION  AT  AN2  <  Y .  X  !  REAL)!  REAL)  EXTERNAL) 


BEGIN  /*  F00T34  */ 


/***************************************************************/ 
/*  EXPRESS  INCREMENTAL  BODY  DISPLACEMENT  IN  BODY  COORDINATES  */ 
/***************************************************************/ 

/***  CALCULATE  INCREMENTAL  BODY  DISPLACEMENT  URT  GROUND  ***/ 

PSIC  S 3  DPSI  *  DT 5 

/***  CALCULATE  MAGNITUDE  OF  VELOCITY  URT  GROUND  ***/ 

VEL  :=  VELX  *  VELX  +  VELY  *  VELY ! 

IF  VEL  <>  0.0  THEN  VEL  !*  SORT <VEL) t 

IF  ABS(DPSI)  >  0.00001 
THEN 

BEGIN 

DXE  :=  VEL/DPSI  *  SIN(PSIC)  ) 

DYE  J=  VEL/DPSI  *  (1.0  -  COS(PSIC))) 

END 

ELSE 

BEGIN 

DXE  1=  VEL  *  DTI 

DYE  {  =  0 . 0  i 

END  > 

/***  ROTATE  DISPLACEMENT  VECTORS  TO  BODY  COORDINATES  ***/ 

ALPHA  :=  ATAN2(  VELY.  VELX  )) 

DXB  :=>  DXE  *  COS  (  ALPHA  > 

-  DYE  *  SIN< ALPHA) ) 

DYB  !»  DXE  *  SIN ( ALPHA ) 

+  DYE  *  CQS( ALPHA) 5 


/************************************/ 

/*  COMPUTE  FOOT  TOUCHDOWN  OFFSETS  */ 
/***********************»************/ 

/***  CALCULATE  BODY  DISPLACEMENT  FROM  MIDSTANCE  TO  TOUCHDOWN  ***/ 
DTI  J=  -0.5  *  ABS  <  SPERIOD  >  )  <  BACKED  UP  TIME  FROM  MIDSTANCE  > 

PSIC1  !=  DPSI  *  DTI)  <  BACKED  UP  ANGLE  > 

IF  ABS ( DPS  I >  >  0.00001 
THEN 

BEGIN 

DXE  5=  VEL  /  DPSI  *  SIN(FSICl)) 

DYE  !=  VEL  /  DPSI  *  (  1.0  -  COS(PSICl)  )) 

END 

ELSE 

BEGIN 

DXE  !=  VEL  *  DTD 

DYE  !=  0.0) 

END) 
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» 


/***  ROTATE  TOUCHDOWN  OFFSET  TO  BOOT  COORDINATES  ***/ 
DXB1  :>  DXE  *  COS ( ALPHA ) 

-  DYE  *  SIN (ALPHA) i 

DYB1  :»  DXE  *  S IN  <  ALPHA ) 

+  DYE  *  COS ( ALPHA  )  I 


RBADC(5. 38.1. TOP BLOCK)!  <  FETCH  PITCHr  ROLL,  t  POWER  FROH  VEHICLE  > 


PPITCH  !=  T0PBL0CKC4]  *  ATTSCALEi 
PROLL  S=  TOPBLOCKC53  *  ATTSCALEi 
POWER  :=  T0PBL0CKC6I  *  PWRSCALEI 
PITCH  ! =  T0PBL0CKL7I  *  ATTSCALEi 
ROLL  5=  TOPBLOCKC83  *  ATTSCALEi 


» 

k 


X 


k 


/mmmttmtttmttmmm*/ 

/*  GENERATE  FOOT  TRAJECTORIES  */ 

/It************************* ******/ 

FOR  I  !=  1  TO  6  DO  t  GENERATE  FOOT  COORDINATE  FOR  LEG  I  > 

BEGIN 

/***  COMPUTE  LEG  PHASE  VARIABLE  ***/ 

LPHASE  {=  PHASE  k  RPHASECII  k  l.Oi 
LPHASE  •  =>  LPHASE  -  TRUNC<  LPHASE  )i 

IF  LPHASE  >  BETA 

THEN  <  LEG  IN  TRANSFER  PHASE  > 

BEGIN 

TPHASE  }=  (LPHASE  -  BETA)  /  (1.0  -  BETA)! 

SUPPORT  !=  SUPPORT  -  Cl  3 »  <  REMOVE  LEG  I  FROM  SUPPORT  SET  > 

IF  <  LEG  AT  TOP  OF  TRANSFER  PHASE  AND  FORCE  NOT  YET  ZEROED  > 

(  (  (PERIOD  >  0.0)  AND  (LPHASE  >  < BETAk 1 > /2 . 0 >  > 

OR  (  (PERIOD  <  0.0)  AND  (LPHASE  <  ( BET Ak 1  ) /2 . 0 )  )  > 

AND  (FZER0CI3  =  FALSE) 

THEN  {  UPDATE  OFFSET  FORCES  FOR  LEG  I  > 

BEGIN 

FRSTCH  J=  (  I  -  l  )  *  3i 

RBADC (  3.  FRSTCH.  FSCALE .  ZEROFORCEX 

FZEROCri  :=  TRUE!  <  FLAG  THAT  LEG  1  IS  UPDATED  > 

ZPTERMC 1 3  !=  O.Oi  <  INITIALIZE  ATTITUDE  CORRECTION  TERM  > 
END! 


<  END  IF  > 


/***  CALCULATE  TIME  LEFT  TILL  TOUCHDOWN  OF  THE  LEG  ***/ 
TRTIME  :=  <  1  -  BETA  )  *  ABS<PERIOD>! 

IF  PERIOD  >  0.0 

THEN  TINEFP  :*  TRTIME  *  (  1.0  -  TPHASE ) 

ELSE  TINEFP  S=  TRTIME  *  TPHASE! 

/***  COMPUTE  THE  BEST  TOUCHDOWN  POINT  ***/ 

XFTHLDCI3  J*  <  NIDSTXC 1 3  -  DXB1  )  *  COS(PSICl) 

+  <  MIDSTYC II  -  DYB1  )  *  SINCPSICl)! 

YFTHLDCI]  ■ ■  -  <  MIDSTXCI3  -  DXB1  )  *  SIN(PSICl) 

+  (  MIDSTYC  1 3  -  DYB1  >  *  COS(PSICl)! 


/*»*  COMPUTE  DESIRED  FOOT  POSITION  ***/ 

IF  DT  <  TIMEFP 
THEN 

BEGIN 

XPDCI3  J=  XPDCI3  +  ( XFTHLDC 1 3  -  XPDCI3)  *  DT/TIMEFF'! 

Ypocn  :*  ypDcn  t  <yfthldcm  -  ypocid  *  dt/timefp! 

ZPDCn  :*  MIDST  Z  -  FOOTLIFT  *  SIN<  TPHASE  *  FI  >! 

END 


ELSE 

BEGIN 

XPDCII  XFTHLDC 1 3  > 
YPDCn  :=  YFTHLDC 1 1 ! 
ZPDCI3  ! =  MIDSTZI 
END  I 


{  END  IF  DT  > 


/***  COMPUTE  DESIRED  FOOT  RATE  ***/ 

XRBCII  *.*  <  XFTHLDCU  -  XPDCII  >  /  TIMEFP} 
YRDCI3  ! ■  (  YFTHLDC 1 3  -  YPDCI3  )  /  TIMEFP! 
ZRDCI3  !=  -  FOOTLIFT  *  PI  *  COS(  PI  *  TPHASE  ) 
/  <  PERIOD  *  <  l  -  BETA  )  >i 


END  <  TRANSFER  PHASE  > 

ELSE  <  FOOT  IN  SUFPORT  PHASE  > 
BEGIN 


SUPPORT  ;«  SUPPORT  +  C  1 3  !  <  INCLUDE  LEG  I  IN  SUPPORT  SET  > 

FZER0CI3  !»  FALSE!  <  FLAG  THAT  FORCE  NOT  ZEROED  THIS  CYCLE 

/***  COMPUTE  ATTITUDE  CONTROL  VARIABLES  ***/ 

ZOFF  :=  -PITCH  «  XPACIJ  t  ROLL  *  YPACI3! 


HO 


IF  ATTITUDE  -  TRUE 
THEN 

BEGIN 

ZRTERH  S=  ATTPOLE  *  ZOFF! 

ZPTERMC I  3  :=  ZPTERHC I 3  +  ZRTERh  *  DT 
END 

ELSE 

BEGIN 

ZPTERHC I  3  :=  O.O. 

ZRTERH  !*  0.05 
END  i 

<  END  IF  ATTITUDE  > 

/***  COHPUTE  DESIRED  FOOT  POSITION  ***/ 

xpdcii  :*  c  xpdcii  -  dxb  >  *  cos<psic> 

+  <  YPDCI3  -  DYB  )  *  SIN(PSIC) f 

YPDCII  -  <  XPDCII  -  DXB  >  *  SIN(PSIC) 

+  <  YPDCII  -  DYB  >  *  COS(PSIC)! 

ZPDCII  5  =  HIDSTZ  +  ZPTERHC 1 3  ! 

/***  COHPUTE  DESIRED  FOOT  RATE  ***/ 

XRDCII  t-  -UELX  +  DPSI  *  YPDCII! 

YRDCII  :•=  -VELY  -  DPSI  *  XPDCII! 

ZRDCII  :=*  ZRTERH! 

END!  /*  SUPPORT  PHASE  */ 

/*  END  IF  LPHASE  #/ 

END!  /*  FOR  I  */ 


/tut*********************************/ 

/*  COHPUTE  OPTIHAL  FORCE  SETPOINTS  *f 

/*************************************/ 

N  : «  0! 

suhx  :»  o.oi 

SUHY  :»  0.0! 

SUHX2  :»  0.0! 

SUHY2  :*  0.0! 

SUHXY  ! *  0.0! 


FOR  I  !»  1  TO  4  DO 

IF  I  IN  SUPPORT  THEN 
BEGIN 

sumx  :=  sumx  +  xpacu! 

SUNY  <  =  SUMY  +  YPAC I  3  i 

SUMX2  :=  SUMX2  +  XPACI1  *  XPACI3! 

SUMY2  J*  SUMY2  +  YPACI3  *  YPAC 1 3 1 

SUMXY  !-  SUMXY  +  XPACI3  *  YPAC  13! 

N  »=  N  +  1 ! 

END!  <  IF  I  IN  SUPPORT  > 

{  END  FOR  I  > 

R  !=  < SUMX  *  SUMY2  -  SUMXY  *  SUMY)  /  (SUMX2  *  SUMY2  -  SUMXY  *  SUMXY)! 

Q  !=  (SUMY  -  SUMXY  *  R)  /  SUMY2I 

S  5«  N  -  ((SUMX  *  SUMY2  -  SUMXY  *  SUMY)  *  R  +  SUMY  *  SUMY)  /  SUMY2 ! 

CGX  :=»  -SIN(  PITCH  )  *  MIDSTZ! 

CGY  ! ■  SIN(  ROLL  )  *  MIDSTZ! 

/***  COMPUTE  FOOT  FORCE  SETPOINTS  ***/ 

FOR  I  :»  1  TO  6  DO 
BEGIN 

XFDCI]  < ■  O.O! 

YFDCI3  :*  0.01 

IF  I  IN  SUPPORT 
THEN 

IF  OPTIMIZATION  »  TRUE 

THEN  ZFDCI3  :■  (  1  -  Q  *  (  YPAC I 3  -  CGY  > 

-  R  t  (  XPACI3  -  CGX  )  )  *  FTOTAL  /  S 
ELSE  ZFDCIJ  :*  FTOTAL  /  N 

ELSE 

ZFDCIJ  !■  0.0! 

END!  <  FOR  I  > 

JSERVO(  MOVE  )»  <  CALL  SERVO  ROUTINE  > 


END!  /*  FOOTPATH  */ 


<*«E  +  «) 


/******  FILE:  SERV34.PAS  ******/ 


/*«*St«***t ****************/ 

/*  PROCEDURE  JSERVO  */ 

**********  **********/ 

/****************************************************************/ 


/* 

PROGRAMMER 

:  DENNIS 

PUGH 

*/ 

/* 

date: 

30-MAR- 

82 

*/ 

/* 

function: 

ACCOMPLISHES  JACOBEAN  SERVO 

CONTROL. 

%/ 

/* 

ACTIVE 

COMPLIANCE  IS  TURNED 

ON  OR  OFF 

*/ 

/* 

BY  THE 

BOOLEAN  SWITCH  'SPRING* 

*/ 

/* 

*/ 

/* 

USER 

GUIDE 

:  THE  CALLING  FORMAT  IS: 

%/ 

/* 

JSERVO ( 

MOVE  >  S 

t/ 

/* 

*/ 

/* 

PROCEDURES 

called: 

RBADCt  RBDAC 

*/ 

/* 

*/ 

/* 

GLOBAL  VARIABLES 

*/ 

/* 

referenced: 

XRDt  YRD>  ZRD 

*/ 

/% 

XPD *  YPD»  ZPD 

*/ 

/% 

XFD.  YFD t  ZFD 

%/ 

/* 

ZEROFORCE.  SPRING 

*/ 

/* 

*/ 

/% 

modified: 

XPA i  YPA  >  ZPA 

%/ 

/% 

XFA  *  YFA»  ZF  A 

*/ 

/* 

XFFi  YFF »  ZFF 

*/ 

/* 

PASSI 

*/ 

/* 

*/ 

/St**************************************************************/ 

PROCEDURE  JSERVO (  MOVE!  BOOLEAN  )f 


CONST 

COMPGAIN 

=  57.4? 

/* 

COMPENSATOR  GAIN 

*/ 

PGAINX  * 
PGAINY  = 
PGAINZ  = 

3.8? 

3.8? 

1.32? 

/* 

RECTILINEAR  POSITION  GAIN 

*/ 

FGAINX  = 
FGAINY  = 
FGAINZ  = 

O.OOOl  ? 
0.0001  ? 
0.165? 

/* 

RECTILINEAR  FORCE  GAIN 

%/ 

FILTPOLE 

=  2.0? 

/* 

POLE  FOR  FORCE  TERM  FILTER 

*/ 

PSCALE  = 

1.571? 

/» 

POSITION  SCALE  FACTOR 

«/ 

RSCALE  =■ 

0.61? 

/* 

RATE  SCALE  FACTOR 

*/ 

ZFSCALE 

=  -1.25? 

/* 

Z-AXIS  FORCE  SCALE  CHANGE 

*/ 

VSCALE  ■ 

10.0? 

/* 

VOLTAGE  SCALE  FACTOR 

*/ 
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I*  . 


I- 

I 


B 


1 

-« 


tc 


Ll 

■ 

12.5641 

/* 

UPPER  LIMB  LENGTH 

*/ 

L2 

a 

17.00$ 

/* 

LOWER  LIMB  LENGTH 

%/ 

L3 

a 

-1 .436  S 

/* 

AZIMUTH  JOINT  OFFSET 

*/ 

LA 

a 

2 . 5  S 

/* 

ELEVATION  JOINT  OFFSET 

*/ 

LS 

a 

2.436S 

/* 

KNEE  JOINT  OFFSET 

*/ 

YHIP 

a 

8.562S 

/* 

Y  DISTANCE  FROM  HIP  TO  C.G. 

*/ 

VAR 


PSI, 

THETA, 

THETA1 , 

THETA2 , 

/* 

JOINT  ANGLES 

*/ 

CPSI, 

CTH , 

CTH1 , 

CTH2 , 

/* 

COSINES  OF  ANGLES 

*/ 

SPSI, 

STH, 

STH1 , 

STH2 , 

/* 

SINES  OF  ANGLES 

*/ 

TNI , 

TN2, 

Dl, 

D2 , 

/* 

INTERMEDIATE  TERMS 

*/ 

All, 

A12, 

/* 

INVERSE 

t/ 

A21, 

A22 , 

A23 , 

/* 

JACOBEAN 

*/ 

A31 , 

A32 , 

A33 , 

/* 

MATRIX 

%/ 

XRC , 

YRC, 

ZRC , 

/* 

RECT.  RATE  COMMANDS 

*/ 

VOUT, 

/* 

MOTOR  OUTPUT  VOLTAGE 

*/ 

YSIGN, 

/* 

LEFT  SIDE  CORRECTION 

*/ 

MAXTIME 

, 

/* 

MAX  DT  FOR  FILTER 

*/ 

DTF , 

/* 

FILTER  DT 

*/ 

XFE. 

YFE. 

ZFE, 

/* 

RECT.  FORCE  ERROR 

*/ 

XFORCE-, 

YFORCE, 

ZFORCE 

S  REALS 

/* 

FOOT  COORD.  FORCES 

*/ 

XHIP 

S  ARRAY6S 

/* 

X  COORD.  HIP  TO  C.G. 

*/ 

I ,  J , 

/* 

LOOP  INDEX 

*/ 

CHANO,  CHAN1 , 

CHAN2 

5  INTEGERS 

/* 

A/D  CHANNEL  POINTERS 

*/ 

FORCE, 

/* 

FORCE  INPUT  ARRAY 

*/ 

POSITION, 

/* 

ACT.  JOINT  POSITIONS 

*/ 

RATE, 

/* 

ACTUAL  JOINT  RATES 

«/ 

RATECOM, 

/% 

COMMAND  JOINT  RATES 

*/ 

VOLT 

S  ARRAY 1 8  S 

/* 

MOTOR  VOLTAGE  OUTPUT 

*/ 

I 


i 


PROCEDURE  RBADC  <  NOCHAN,  FRSTCHS  INTEGERS  SCALES  REALS  VAR  INDATA !  ARRAY  1 8  >S 
EXTERNALS 


PROCEDURE  RBDAC <  NOCHAN,  FRSTCHS  INTEGERS  SCALES  REALS  VOLTS  ARRAY  1 8  >1 
EXTERNALS 


FUNCTION  SIGN<  XS  READS  REALS 


EXTERNALS 


I 
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BEGIN  /*  JSERVO  */ 


XHIPC13  •  *  22.75! 
XHIPC43  O.OI 


XH1PC23  !-  22.75! 
XHIPC53  S=  -22.75! 


XHIPC33  0.0! 
XHIPC63  • “-22.75! 


/*  CONDITION  DT  FOR  FAST  FILTER  */ 

MAXTIHE  J=  1.0  /  <  FILTPOLE  *  4.0  )! 

IF  DT  >  MAXTIME  <  THEN  FILTER  DEGENERATES  > 

THEN  DTF  S*  MAXTIME 
ELSE  DTF  J*  DT! 

FOR  I  5*  1  TO  6  DO 
BEGIN 

IF  I  IN  £  1 « 3 » 53 

THEN  YSIGN  S=  -1.0 
ELSE  YSIGN  5  =  1.0! 

CHANO  !»  <I-1>  *  3! 

CHAN1  !»  CHANO  4  1! 

CHAN2  ! =  CHANO  4  2! 

RBADC ( 3  v  36+CHANO.  PSCALE »  POSITION)!  <  READ  JOINT  POSITION  > 
RBADC ( 3 r  CHANO »  FSCALEf  FORCE)!  <  READ  FOOT  FORCES  > 

THETA2  5»  P0SITI0NCCHANO3 ! 

THETA 1  S*  POSITION CCHAN1 3 ! 

PSI  5»  P0SITI0NCCHAN23! 

/*  COMPUTE  TRUE  FORCES  IN  LEG  COORDINATES  */ 

XFORCE  :»  F0RCECCHAN03  -  ZER0F0RCECCHAN03 ! 

YFORCE  ! ■  -< F0RCECCHAN1 3  -  ZER0F0RCECCHAN1 3 ) ! 

ZFORCE  I*  ZF SCALE  *  !F0RCEtCHAM23  -  2ER0F0RCETCHAN23 ) ! 

/*  COMPUTE  I  SAVE  ALL  NECESSARY  TRIG.  FUNCTIONS  */ 

CPSI  5=  COS(PSI ) ! 

SPSI  !»  S IN < PS I ) f 
CTH1  i-  COS(THETAl)! 

STH1  :=  SIN<  THETA1 ) ! 

CTH2  ! ■  COS ( THETA2 ) ! 

STH2  !«=  S  IN<  THETA2  )  ! 

THETA  5»  THETA1+THETA2! 

STH  S*  SIN ( THETA ) I 
CTH  !*  COS  <  THETA ) ! 

TNI  ! =  LI*CTH1+L5*STH1+L2*STH! 

D1  L4+TN1 ! 

D2  :«  L2*<L1*CTH2-L5*STH2> ! 

TN2  !=  TN1/D2! 

/*  COMPUTE  THE  ACTUAL  RECTILINEAR  FOOT  POSITION  */ 


XPACI3 

YPACI3 

ZPACI3 


=  DUSPSI  -  L34CPSI  +  XHIPEI3! 

»  ( D 1 *CFS I  +  L34SPSI  +  YHIP)  *  YSIGN! 
=  -L1*STH1  4  L2*CTH  4  L54CTH1  ! 


/*  ROTATE  FORCES  TO  BODY  COORDINATES  */ 

XFACI3  :»  SPSI*CTH  «  XFORCE 

-  YSIGNtCPSI  *  YFORCE 

+  SPSItSTH  *  ZFORCE ) 

YFACI3  5=  YSIGN*CPSI*CTH  *  XFORCE 

+  STH  *  YFORCE 

+  YSIGNtCPSItSTH  *  ZFORCE  ) 

ZFACI3  !=  -STH  *  XFORCE 

t  CTH  *  ZFORCE  ) 


IF  HOVE  -  TRUE  THEN  <  SERVO  THE  HEXAPOD  > 

BEGIN 

IF  SPRING  *  FALSE  <  THE  FORCE  TERH  MUST  BE  ZEROED  > 

THEN 

BEGIN 

xFFtn  :=>  o.o» 

YFFC  1 3  :=»  0.0) 

ZFFCI3  O.O) 

END 

ELSE 

IF  PASS1  ■  TRUE  <  THE  FILTER  MUST  BE  INITIALIZED  > 

THEN 

BEGIN 

XFFCI3  :*  0.0) 

YFFC 1 3  1=  0.0) 

ZFFCI3  J»  0.0) 

END 

ELSE 

BEGIN 

/*  COHPUTE  THE  FORCE  ERRORS  */ 

XFE  5  =  XFDtl3  -  XFACI3) 

YFE  t*  YFDCI3  -  YFACI3) 

ZFE  5=  ZFDCI3  -  ZFACI3) 

/*  LIMIT  FORCE  ERRORS  TO  GIVE  4*  MAX.  DEFLECTION  */ 
IF  ABS<XFE)  >  4.0  *  PGAINX/FGAINX 

THEN  XFE  :*  4.0  *  PGAINX/FGAINX  *  SIGN(  XFE  )) 

IF  ABS  <  YFE )  >  4.0  *  PGAINY/FGAINY 

THEN  YFE  i»  4.0  *  PGAINY/FGAINY  *  SIGN)  YFE  )) 

IF  ABS ( ZFE )  >  4.0  *  PGAINZ/FGAINZ 

THEN  ZFE  5=  4.0  *  PGAINZ/FGAINZ  *  SIGN<  ZFE  )) 


/%  LOU-PASS  FILTER  THE  FORCE  ERRORS  */ 

XFFCI3  ! =  XFFCI3  +  FILTPOLE  *  <XFE  -  XFFCI3)  *  DTF ) 
YFFC 1 3  J=  YFFC  1 3  +  FILTPOLE  *  (YFE  -  YFFC  1 3  )  *  DTF) 
ZFFCI3  !=  ZFFCI3  +  FILTPOLE  *  (ZFE  -  ZFFC13)  *  DTF) 


ENDS  <  ELSE  > 


<  END  IF  PASS1  > 

<  END  ELSE  > 

<  END  IF  SPRING  > 

/*  COMPUTE  RECTILINEAR  FOOT  RATE  COMMAND  */ 

XRC  «■  XRDCI3  +  PGAINX  *  <XPDCI3  -  XPAIII)  +  FGAINX  *  XFFCI3S 

YRC  J»  (YRDCI3  +  PGAINY  *  (YPDCII  -  YPACI3)  +  FGAINY  *  YFFCII)  *  YSIGNS 

ZRC  »■  ZRDCI3  +  PGAINZ  *  (ZPDCI3  -  ZPACI3)  +  FGAINZ  *  ZFFCI3S 

/*  COMPUTE  INVERSE  JACOBIAN  MATRIX  */ 

All  CPSI/D1 I 
A 12  S»  -SPSI/D1S 

A21  I-  -<L2*SPSI*STH)/D2+L2*L3*STH*CPSI/'D1/D2S 

A22  S*  -L2*STH*CPSI/D2-L2*L3*STH*SPSI/Di/D2S 

A23  5-  -L2*CTH/D2S 

A31  S*  <SPSI-L3*CPSI/D1)*TN2S 

A32  5»  < CPSI+L3FSPS I/D1)*TN2S 

A33  S*  <-Ll*STHl+L2*CTH+L5*CTHl>/D2» 

/*  COMPUTE  JOINT  RATE  COMMAND  */ 

RATECOMCCHAN23  S  =  A11*XRC  +  A12*YRCS 
RATECOHCCHAN13  :■  A21*XRC  +  A22*YRC  +  A23*ZRC$ 

RATEC0MCCHAN03  i=  A31*XRC  +  A32*YRC  +  A33*ZRCS 


/***  RATE  SERVO  SECTION  ***/ 

RBADC ( 3 • 18+CHANO . RSCALE » RATE ) i  <  FETCH  ACTUAL  JOINT  RATES  > 

FOR  J  S=  CHANO  TO  CHAN2  DO 
BEGIN 

VOUT  :»  COMPGAIN  *  (RATECOMCJ3  -  RATEI J] ) S 
IF  VOUT  >  9.8  THEN  VOUT  :=  9.8 

ELSE  IF  VOUT  <  -9.8  THEN  VOUT  S=  -9.8! 

VOLTCJ3  !=  VOUT  S 

RBDAC (liJ>VSCALE>VOLT)S  <  OUTPUT  THE  VOLTAGE  > 

ENDS  {  DO  > 

ENDS  <  IF  MOVE  > 

ENDS  {  DO  > 

PASS  1  :=  FALSES  <  FLAG  THAT  FILTERS  ARE  INITIALIZED  > 

I  JSERVO  > 
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URITELNI 

URITELNI  CHR ( 27 )  .  '*6  NORMALIZING 

JSERVOI  NOMOVE  >»  <  FETCH  ACTUAL  FOOT  POSITIONS  > 

TURNON!  <  TURN  ON  MOTOR  POWER  > 

FOR  FOOT  1  TO  6  DO  <  INITIALIZE  COORDINATES  > 

BEGIN 

xcooRDtFoon  : =  xpacfooti; 

YCOORDCFOOT3  {*  YPACFOOT3; 

ZCOORDCFOOT3  S=  MIDSTZj 

end; 

FOOTLINE (  3.0.  XCOORD.  YCOORD.  ZCOORD  );  <  MOVE  TO  MIDSTANCE  Z  > 


FOR  FOOT  5=  1  TO  6  DO 

IF  FOOT  IN  C  1.  4.  5  3 

THEN  ZCOORDCFOOT3  !=  MIDSTZ  -  5.0; 

FOOTLINE <  5.0.  XCOORD.  YCOORD.  ZCOORD  >;  {  MOVE  LEG  SET  1  UP  > 

FOR  FOOT  !=  1  TO  6  DO 

IF  FOOT  IN  C  1,  4.  5  3  THEN 
BEGIN 

XCOORDCFOOT3  J*  MIDSTXCF00T3 f 
YC00RDCF00T3  S=  MIDSTYCF00T3 ; 

end; 

FOOTLINE <  5.0.  XCOORD.  YCOORD.  ZCOORD  >;  <  MOVE  LEG  SET  1  OVER  > 

FOR  FOOT  :=  1  TO  6  DO 

IF  FOOT  IN  C  1.  4.  5  3  THEN 
BEGIN 

FRSTCH  (  FOOT  -  l  >  *  31  {  COMPUTE  FIRST  A/D  CHANNEL  > 

RBADC (  3.  FRSTCH.  FSCALE.  ZEROFORCE  >;  <  READ  FORCE  OFFSET  > 
FZER0CF00T3  !»  TRUE;  <  FLAG  THAT  ZEROFORCE  UPDATED  > 

ZC00RDCF00T3  :=  MIDSTZ; 

end; 

FOOTLINE (  5.0.  XCOORD.  YCOORD.  ZCOORD  >;  <  MOVE  LEG  SET  1  DOUN  > 

FOR  FOOT  !=  I  TO  6  DO 

IF  FOOT  IN  C  2.  3.  6  3 

THEN  ZCOORDCFOOT 3  !=  MIDSTZ  -  5.0; 

FOOTLINE <  5.0.  XCOORD.  YCOORD.  ZCOORD  >;  {  MOVE  LEG  SET  2  UP  > 

FOOTLINE l  5.0.  MIDSTX.  MIDSTY.  ZCOORD  );  <  MOVE  LEG  SET  2  OVER  > 


THEN 


FOR  FOOT  !>  1  TO  i  DO 

IF  FOOT  IN  C  2,  3i  6  3 
BEGIN 

FRSTCH  S ■  <  FOOT  -  1  )  *  38  {  COMPUTE  FIRST  A/D  CHANNEL  > 

RBADC <  3 >  FRSTCH r  FSCALE »  ZEROFORCE  >8  <  READ  FORCE  OFFSET  > 
FZEROCFOOT3  TRUE 8  <  FLAG  THAT  ZEROFORCE  UPDATED  > 

ZCOORDCFOOT3  !  *  MIDSTZ8 
END  8 

FOOTLINE <  5.0.  MIDSTX.  MIDSTY >  ZCOORD  >8  <  MOVE  LEG  SET  2  DOWN  > 

HALT  8 

URITELN8 

URITELNT  CHR ( 27 ) »  '  NORMALIZATION  COMPLETE' >8 
URITELN8 

END8  /*  NORMALIZE  */ 
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/xtttttxttttttttxxxtxxttxtttttx/ 

/*  procedure:  initialize  */ 

/**«*t*t***t *******************/ 

/*st*ttt**«t*****t*t*******t*****t**t«»******t*******t*t**t***t<s/ 


/* 

programmer: 

DENNIS 

PUGH 

*/ 

/* 

date: 

20-APR 

-82 

$/ 

/* 

funcion: 

RAISE 

LEGS  TO  WALKING  POSITION 

%/ 

/* 

*/ 

/* 

user  guide: 

THE  CALLING  FORMAT  IS: 

*/ 

/» 

INITIALIZE! 

*/ 

/* 

*/ 

/* 

%/ 

/* 

PROCEDURES 

called: 

FOOTLINE.  HALT.  TURNON 

*/ 

/* 

%/ 

/* 

GLOBAL  VARIABLES 

*/ 

/* 

referenced: 

BETA.  RPHASE 

*/ 

/* 

X/ 

/* 

modified; 

PHASE.  SUPPORT.  ZPTERM 

%/ 

/* 

%/ 

/****************************************************************/ 

PROCEDURE  INITIALIZE! 

VAR  FOOT 

TPHASE 
ZINIT 

PROCEDURE  HALT!  EXTERNAL! 

PROCEDURE  FOOTLINE (  VELHAXS  REAL!  XCOORDr  YCOORD.  ZCOORD!  ARRAYS  >!  EXTERNAL! 
PROCEDURE  TURNON!  EXTERNAL! 


t  INTEGER! 
:  REAL! 

:  ARRAYS! 


BEGIN 

URITELNi 

WRITELN<  CHR  <  27 )  »  '*6  INITIALIZING  ...')! 

SUPPORT  t*  C  1..S  Ji  <  START  WITH  ALL  FEET  IN  SUPPORT  PHASE  > 
TURNON!  <  TURN  ON  HOTOR  POWER  > 
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/***  COHP’.'TE  DESIRED  INITIAL  FOOT  HEIGHTS  ***/ 

FOR  FOOT  :<  1  TO  i  DO 
BEGIN 

ZPTERHC  FOOT  3  :*  O.OI  <  INITIALIZE  ATTITUDE  CORRECTION  TERM  > 

IF  RPHASEC  FOOT  3  <  BETA 
THEN 

Z1NITC  FOOT  3  S=  HIDSTZ 

ELSE 

BEGIN 

SUPPORT  S-  SUPPORT  -  CF0QT3  *  <  REHOVE  FOOT  FROh  SUPPORT  SET  > 
TPHASE  5=  <  RPHASEC  FOOT  3  -  BETA  )  /  <  1.0  -  BETA  >» 

ZINITC  FOOT  3  S-  HIDSTZ  -  FOOTLIFT  *  SIN<  TPHASE  *  PI  >J 
END  > 

<  END  IF  > 

END.  <  FOR  FOOT  > 

FOOTLINE <  5.0.  HIDSTX.  HIDSTY.  ZINIT  >1  {  RAISE  LEGS  IN  TRANSFER  PHASE  > 

HALT } 

PHASE  S*  0.01  <  INITIALIZE  KINEHATIC  CYCLE  PHASE  > 

HRITELN1 

URITELN<  CHR<  27 ) .  '#6  INITIALIZATION  COHPLETE  '  >  1 
URITELNf 

END I  /*  INITIALIZE  */ 
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/**********************/ 

/*  PROCEDURE  UPDOUN  */ 
/it********************/ 

/MM************************************************************/ 


/*  PROGRAMMERS  DENNIS  PUGH  */ 
/*  DATES  20-APR-82  */ 
/*  FUNCTIONS  CHANGE  THE  BODY  ELEVATION  */ 
/*  */ 
/*  USER  GUIDES  THE  CALLING  FORMAT  ISi  UPDOUNC  HEIGHT  >S  */ 
/*  WHERE  ’HEIGHT*  IS  THE  DESIRED  BODY  ELEVATION  */ 
/*  */ 
/*  */ 
/*  PROCEDURES  CALLED!  FOOTLINE »  HALT t  TURNON  */ 
/*  */ 
/*  GLOBAL  VARIABLES  */ 
/*  REFERENCED!  HIDSTX,  MIDSTY  */ 
/*  */ 
/*  MODIFIED!  NONE  */ 
/*  */ 


/****************************************************************/ 

PROCEDURE  UPDOWN (  HEIGHT  S  REAL  IS 


VAR  FOOT  S  INTEGER S  /*  FOOT  INDEX  */ 

ZCOOPD  S  ARRAY6!  /*  FOOT  Z  COORDINATES  */ 


PROCEDURE  FOOTLINE  <  VELMAXS  REAL!  XCOORD,  YCOORD,  ZCOORDS  ARRAY6  IS 
EXTERNALS 

PROCEDURE  HALTS  EXTERNALS 
PROCEDURE  TURNONS  EXTERNALS 
BEGIN 

TURNONS  {  TURN  ON  MOTOR  POWER  > 

FOR  FOOT  S=  1  TO  6  DO  ZCOORDCFOOT I  !=  HEIGHTS 

FOOTLINE<  3.0.  MIDSTX.  MIDSTY,  ZCOOkD  >S  <  MOVE  TO  DESIRED  ELEVATION  > 
HALT! 


ENDS 


<  UPDOUN  > 


<**E+*> 


X******* 


FILE*.  LINE34 .  PAS  ttttttt/ 


X **************************** X 
X*  PROCEDURE?  FOOTLINE  */ 
/****************************/ 

X**************************** ****** ******************** t»*«******X 


/*  programmer: 

X*  DATE? 

X* 

X*  FUNCTION? 

X* 

X* 

X* 

X*  USER  GUIDE? 

X* 

X* 


DENNIS  PUGH 
20-APR-C2 

MOVE  FEET  FROM  PRESENT  POSITIONS  TO 
SPECIFIED  POSITIONS  IN  STRAIGHT  LINES 
HITH  ALL  MOTIONS  COMPLETED  SIMULTANEOUSLY 

THE  CALLING  FORMAT  IS? 

FOOTLINE (  VELMAX,  XCOORD.  YCOORD.  ZCOORD  >) 


X* 

PROCEDURES  CALLED? 

JSERVO 

*X 

X* 

*x 

X* 

GLOBAL  VARIABLES 

*x 

X* 

referenced: 

XPA. 

YPA. 

ZPA 

*x 

X* 

*/ 

/•* 

MODIFIED? 

XPD. 

YPD. 

ZPD 

*x 

X* 

XRD. 

YRD . 

ZRD 

*x 

X* 

DT. 

SPRING 

*x 

/************************************************************** **x 

PROCEDURE  FOOTLINE (  VELMAX?  REAL)  XCOORD.  YCOORD.  ZCOORD?  ARRAY6  )f 


CONST  GAIN  »  2.0) 


X*  POSITION  ERROR  TO  RATE  GAIN  %/ 


VAR  ERRORX.  ERRORY,  ERRORZ 
?  ARRAY6) 


X*  POSITION  ERROR  FROM  SETPOINT  *X 


PTIME.  LTIME. 
MAXERR 


?  REAL) 


?  INTEGER) 


X*  TEMPORARY  STORAGE  FOR  TIME  */ 
X*  MAXIMUM  OF  COORDINATE  ERRORS  *X 


X*  FOOT  INDEX 


PROCEDURE  JSERVO <  MOVE!  BOOLEAN  ))  EXTERNAL) 
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BEGIN 

SPRING  S»  FALSE S  -C  DISABLE  ACTIVE  COMPLIANCE  > 
JSERVO<  NOMOVE  >)  <  FETCH  ACTUAL  POSITIONS  > 

FOR  FOOT  5*  1  TO  6  DO  <  INITIALIZE  DESIRED  POSITIONS  > 
BEGIN 

XPDCFOOT3  i*  XPACFOOT3 1 
YPDCFOOT3  S  *  YPACFOOT3S 
ZPDCFOOT3  t=  ZPACFOOT3! 

end; 


LTIME  i »  TIMES 

dt  :*  o.oi; 

REPEAT 

MAXERR  :*  o.o; 

FOR  FOOT  :=  1  TO  6  DO 
BEGIN 

ERRORXCFOOT!  *.=  XCOORDCFOOT3  -  XPACFOOT3  S 
ERRORYCFOOT3  :  =  YCOORDCFOOT3  -  YPACFOOT3S 

errorzcfoot:  !«  zcoordcfoot!  -  zpacfootds 

IF  ABS (  ERRORXCFOOT 3  >  >  MAXERR  THEN  MAXERR  !  =  ABS<  ERRORXCFOOT!  ) 

IF  ABS <  ERRORYCFOOT3  )  >  MAXERR  THEN  MAXERR  !=  ABS<  ERRORYCFOOT3  ) 

IF  ABS <  ERRORZCFOOT3  )  >  MAXERR  THEN  MAXERR  t=  ABS(  ERRORZCFOOT3  ) 

ENDS  /*  FOR  FOOT  */ 


FOR  FOOT  !>  1  TO  i  DO 
BEGIN 

IF  <  GAIN  *  MAXERR 
THEN  BEGIN 

XRDCFOOT3 
YRDCFOOT  3 
ZRDCFOOT  3 
END 

ELSE  BEGIN 

XRDCFOOT3 

YRDCFOOT3 

ZRDCFOOT3 

END) 


)  <  VELMAX  <  INCHES  PER  SECOND  > 

!=  GAIN  *  ERRORXCFOOT  3  S 
<  *  GAIN  *  ERRQRYCFOOT] S 
{a  GAIN  *  ERRORZCFOOT3 ) 


S=  VELMAX  *  ERRORXCFOOT  3  /  MAXERR! 
!=  VELMAX  *  ERRORYCFOOT3  /  MAXERR! 
VELMAX  *  ERRORZCFOQT3  /  MAXERR! 


XFDCFOOT3  !=  XPDCFOOT3  +  XRDCFOOT3  *  DT! 
YPDCFOOT3  ! =  YPDCFOOT3  +  YRDCFOOT J  *  DT! 
ZPDCFOOT  3  !=  ZPDCFOOT3  +  ZRDCFOOT  3  *  DT! 

END)  /*  FOR  FOOT  %/ 


JSERVO  <  MOVE  )  S 


PTIME  1=  TIME  I 

DT  :«  (PTIME  -  LT1ME)  *  3600.05 

LTIME  • 3  PTIME5 

UNTIL  MAXERR  <  0.55 


FOR  FOOT  :»  1 
BEGIN 
XRDCF00T3 
YRDCFOQT] 
ZRDCF00T3 


TO  6  DO  <  CLEAN  UP  GLOBAL  VARIABLES  > 

i  =  0.05 
i  =  0.05 
i=  0.05 


XPDCF00T3  i=  XC00RDCF00T3  5 
YPDCF00T3  i=  YCOORDCFOOT ] 5 
ZPDCF00T3  i=  ZCOORDCFOOn; 
END  5 


ENDS  /*  FOOTLINE  */ 


/**************************/ 

/*  PROCEDURE*.  HALT  */ 
/**************************/ 

/****************************************************************/ 


/*  programmer:  denhis  pugh  */ 

/*  DATES  20-APR-82  */ 

/*  */ 

/*  function:  stops  all  motion  of  hexapod  */ 

/*  user  guide:  the  calling  format  is:  */ 

/*  halt:  */ 

/*  */ 

/*  procedures  called:  rbdac  */ 

/*  */ 

/*  GLOBAL  variables  */ 

/*  referenced:  none  */ 

/*  */ 

/*  modified:  velx.  vely.  dpsi  */ 

/*  NVELX >  NVELY.  NDPSI  */ 

/*  */ 


/************************************** *************«************/ 

procedure  halt: 
const  vscale  =  10. o: 

VAR  JOINT  UNTEGERi  /*  JOINT  INDEX  */ 

VOLT  :ARRAY18!  /*  OUTPUT  VOLTAGES  */ 

PROCEDURE  RBDAC<  NOCHAN.  FRSTCHt  INTEGER.  SCALE:  REAL?  VOLT*.  ARRAY  IB  >  *. 
EXTERNAL! 

PROCEDURE  TURNOF!  EXTERNAL! 

BEGIN 

FOR  JOINT!-  0  TO  17  DO 
VOLTC JOINT ]  :=o.o; 

RBDAC <  18.  0.  VSCALE.  VOLT)!  /*  TURN  OFF  VOLTAGE  TO  ALL  MOTORS  */ 

TURNOF!  /*  TURN  OFF  MOTOR  POWER  */ 

/***  INITIALIZE  VELOCITY  VECTORS  ***/ 

NVELX  :=  0.0!  NVELY  :=  0.0!  NDPSI  !=  0.0! 

VELX  :=  0.0!  VELY  !=  0.0!  DPSI  !=  0.0! 

URITELNi 

WRITELN!  CHR ( 27 ) . ' #6 - HEXAPOD  STOPPED - ')! 

END!  /*  HALT  */ 
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<**E+*> 


/**««**  FILES  L1BR34.PAS  ******/ 


/****» tt*X*t**t **********/ 

/*  FUNCTIONS  ATAN2  */ 

/«***«»**»*** t*t***tt*t**/ 

/**t****t*t***ttt***Stt*t*tt**« tit********************************/ 


/*  programmer:  dennis  pugh  */ 

/*  DATE:  20-APR-82.  */ 
/*  function:  implement  the  four  -  QUADRANT  */ 
/*  INVERSE  TANGENT  FUNCTION  */ 
/*  */ 
/*  user  guide:  the  calling  format  is:  */ 

/*  ATAN2(Y*  X>?  */ 
/*  WHERE i  Y  IS  THE  SIDE  OPPOSED  TO  THE  ANGLE  */ 
/%  X  IS  THE  SIDE  ADJACENT  TO  THE  ANGLE  */ 

/*  */ 


/«***************************************************************/ 

FUNCTION  ATAN2I Y •  X  l  REAL)  :  REAL) 


FUNCTION  SIGN<  X!  REAL  )‘.  REAL)  EXTERNAL) 


BEGIN 

IF  ABS<X)  >  O.OOOOl 

THEN  <  X  IS  NON-ZERO  > 

IF  X  >  0.0 
THEN 

ATAN2  S-  ARCTAN ( Y/X  > 

ELSE 

ATAN2  S»  ARCT  AN ( Y/X  >  +  PI  *  SIGN<  Y  ) 
<  END  IF  X  > 

ELSE 

ATAN2  :»  PI  /  2.0  *  SIGN(  Y  >  ) 


END) 


/*  ATAN2  */ 


/***********************/ 

/*  function:  sign  */ 
/***********************/ 

/****************************************************************/ 


/* 

progranner: 

DENNIS  PUGH 

*/ 

/* 

date: 

20-APR-82 

t/ 

/* 

function: 

IMPLEMENT  THE  SIGNUM  FUNCTION 

*/ 

/* 

*/ 

/* 

user  guide: 

THE  CALLING  FORMAT  IS: 

*/ 

/t 

SIGN<  X  )J 

%/ 

/* 

*/ 

/it**************************************************************/ 

FUNCTION  SIGN'  X  !  REAL  )  J  REAL  » 

BEGIN 

IF  X  >*  0.0 

THEN  SIGN  :«  1.0 
ELSE  SIGN  :  =  -1.0 

END* 
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<*»E  +  *> 


/«****«  FILES  DLNK34  > PAS  ******/ 


/***********************/ 

/*  PROCEDURES  RBADC  */ 
/**********«*«**********/ 

/****************************«**** *******************************/ 


/*  programmer:  denmis  pugh  */ 

/*  DATE:  20-APR-B2  */ 

/*  function:  read  in  input  variables:  force,  rate,  t  */ 

/*  POSITION.  */ 

/*  */ 

/%  user  guide:  the  calling  format  is:  */ 

/*  RBADC (  NOCHAN.  FRSTCH.  SCALE.  INDATA  >5  */ 

/*  where:  */ 

/*  NOCHAN  IS  THE  NUMBER  OF  CHANNELS  TO  BE  READ  */ 

/*  FRSTCH  IS  THE  FIRST  CHANNEL  TO  BE  READ:  */ 

/*  0  FOR  READING  FORCE  VARIABLES  */ 

/*  18  FOR  READING  JOINT  RATES  %/ 

/*  36  FOR  READING  JOINT  ANGLES  */ 

/*  SCALE  IS  THE  INPUT  SCALING  FACTOR  */ 

/*  INDATA  IS  THE  INPUT  BUFFER  */ 

/*  */ 

/«  */ 

/*  PROCEDURES  CALLED:  NONE  */ 

/*  «/ 

/*  GLOBAL  VARIABLES:  NONE  */ 

/%  */ 


/*«****«*********************(***********************************/ 


PROCEDURE  RBADC (NOCHAN .  FRSTCH!  INTEGERS  SCALE!  REALS  VAR  INDATA:  ARRAY18) S 

VAR  I,  J  /*  DATA  POINTERS  */ 

INTEGERS 

ADPORT  ORIGIN  166000B  /*  LOCATION  OF  DATA  LINK  SHARED  MEMORY  */ 

!  ARRAY!  0..63  3  OF  INTEGER! 


BEGIN  /*  RBADC  (/ 

J  !=  FRSTCH  MOD  185  /*  OFFSET  FROM  START  OF  VARIABLE  TYPE  */ 

FOR  I  !=  J  TO  <J  +  NOCHAN  -  1 )  DO  {  CONVERT  t  SCALE  DATA  > 

INDATAC I J  !=  ( 777B  -  ADPORT!  FRSTCH-J  +13)/  777B  *  SCALE! 


ENDS 


/*  RBADC  */ 


/******«****************/ 
/*  procedure:  rbdac  */ 
/*******************««**/ 


/****************************************************************/ 

/*  programmer:  dennis  pugh  */ 

/*  DATE:  20-APR-82  */ 

/*  function:  send  output  voltages  to  data  link  */ 

/*  */ 

/*  user  guide:  the  calling  forhat  is:  */ 

/*  RBDAC (  NOCHAN  r  FRSTCH*  SCALE  >  OUTDATA  )  */ 

*  where:  */ 

/*  NOCHAN  IS  THE  NUMBER  OF  OUTPUT  CHANNELS  */ 

/*  FRSTCH  IS  THE  FIRST  CHANNEL  TO  BE  OUTPUT  */ 

/«  SCALE  IS  THE  OUTPUT  SCALING  FACTOR  */ 

/*  OUTDATA  IS  THE  OUTPUT  BUFFER  */ 

/*  */ 

/*  */ 

/*  PROCEDURES  CALLED:  NONE  */ 

/*  */ 

/*  GLOBAL  VARIABLES:  NONE  */ 

/*  */ 

/**«**(****«***********«*«***************************************/ 

PROCEDURE  RBDAC ( NOCHAN  » FRSTCH :  INTEGER:  SCALE!  REAL >  OUTDATA:  ARRAY18) t 
CONST  READY  -  200BI 

VAR  I,  /*  DATA  POINTER  */ 

TEMP  /*  TEMPORARY  STORAGE  FOR  OUTPUT  */ 

:  integer: 

STATUS  ORIGIN  164244B  /*  DATA  LINK  STATUS  WORD  */ 

:  integer: 

OUTPORT  ORIGIN  166200B  /*  DATA  LINK  OUTPUT  PORTS  %/ 

:  ARRAYCO.  .  173  OF  INTEGER: 

BEG  ' N  /*  RBDAC  */ 

FtP  I  !=  FRSTCH  TO  (FRSTCH  +  NOCHAN  -  1)  DO 
BEGIN 

TEMP  !=  1 77B  -  ROUND(OUTDATACI3  *  177B  /  SCALE): 

WHILE  (STATUS  AND  READY)  =  0  DO  <  WAIT  UNTIL  DATA  LINK  READY  }f 
OUTPORTC 1 3  :*  temp: 

end:  /*  do  */ 

end:  /*  RBDAC  */ 
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PROCEDURE  CLOCK  IN  IT ; 


VAR 

STATUS 

ORIGIN 

170404B 

:  integer; 

PRELOAD 

ORIGIN 

170406B 

:  integer; 

CLOCK 

ORIGIN 

17041 AB 

:  integer; 

BEGIN 

PRELOAD  I-  0>  <  USE  PULL  COUNTER  INTERVAL  > 

STATUS  :=>  4 1 3B  f  {  ENABLE  COUNTER  AT  100  Hz  > 
LASTCLOCK  !*  CLOCK  5 
TOTALCLOCK  S*  Of 
END! 


FUNCTION  DELTATIME  1  REAL  t 

VAR  CLOCK  ORIGIN  170416B  5  INTEGER; 

nouclock  :  integer; 

NUMTICKS  :  integer; 

begin 

NOUCLOCK  5*  clock; 

NUMTICKS  :=»  (  NOUCLOCK  -  LASTCLOCK  >  AND  377B; 
TOTALCLOCK  .**  TOTALCLOCK  +  NUMTICKSi 
LASTCLOCK  {*  NOUCLOCK; 

DELTATIME  i  =  NUMTICKS  /  100. Of 


end; 


(*«£+*> 


/**»**«  FILE:  P0WR34.PAS  »***»*/ 

/I***********************************************************************/ 


/*  */ 

/*  programmer:  dennis  pugh  */ 

/*  DATE?  20-APR-B2  */ 

/*  */ 

/*  GLOBAL  VARIABLES!  NONE  */ 

/*  */ 

/*  THIS  FILE  CONTAINS  SUBROUTINES  WHICH  UTILIZE  THE  SELF-DIAGNOSTIC  */ 

/*  CAPABILITIES  AND  THE  REMOTE  POWER  CONTROL  CAPABILITIES  OF  THE  */ 

/*  DIGITAL  DATA  LINK.  THE  EXTERNALLY  USEFUL  ROUTINES  ARE  *  TESTON*  t  %/ 

/*  *  TURNOF  * »  AND  'TURNON*.  NONE  OF  THESE  REQUIRE  ARGUMENTS.  */ 

/*  */ 


/**t«**t***t****t****t*»****t******************t**t******t *****«*********/ 


/******>*****************************************************************/ 

PROCEDURE  PWAIT f  /*  A  ROUTINE  WHICH  WAITS  UNTIL  ALL  FEEDBACK  CHANNELS  */ 

/*  HAVE  BEEN  UPDATED  AFTER  A  CHANGE  IN  STATUS  */ 

VAR  NOW : REAL S 
BEGIN 

NOW  **TIME : 

WHILE  <  (TIME-N0WX3600.  )  <  0.004  <*  SECONDS  *>  DO  <*  NOTHING  *>  t 
END>  /*  PWAIT  */ 

/**M***M*»***t«*t**t***t  *********************************************  t/ 


/************************************************************* ***********/ 


/*  */ 

/*  TURNOFS  */ 
/%  (TURN  OFF)  IS  A  ROUTINE  WHICH  TURNS  OFF  THE  POWER  TO  THE  */ 
/*  SERVO  MOTORS.  IT  SHOULD  BE  USED  BEFORE  EXITING  THE  CONTROL  */ 
/*  PROGRAM*  AND  ADDITIONALLY*  IT  SHOULD  BE  CALLED  WHENEVER  THE  */ 
/*  HEXAPOD  IS  NOT  ACTUALLY  SERVOING.  THIS  WILL  INCREASE  SAFETY  */ 
/*  AND  REDUCE  THE  RISK  OF  DAMAGE  TO  THE  HEXAPOD.  THE  EFFECTS  */ 
/*  OF  CALLING  TURNOF  CAN  BE  REVERSED  BY  CALLING  TURNON.  */ 
/*  */ 


/ft**********************************************************************/ 

PROCEDURE  TURNOF* 

(**C  COMST AT ="0166244  (ADDRESS  OF  COMMAND  t  FORWARD  STATUS 

POWER1*" 01 662 4 6  (ADDRESS  OF  POWER  COMMAND  WORD 

.MACRO  SEND  Y*Z*?T 
TS  TSTB  COMSTAT 

BPL  T 

MOV  Y.Z 

.ENDM 

*) 

BEGIN 

<**C  SEND  «0*C0MSTAT  (COMMAND  NORMAL  MODE 

SEND  42 • POWER  (TURN  OFF  MOTOR  POWER 

*> 

END*  /*  PROCEDURE  TURNOF  */ 

/*t**tt*******t*«t**t*tt **************************** t «****«*« t*t>*t*tt***/ 


(CHECK  IF  FORWARD  PATH  BUSY 
(SEND  DATA 


1  54 


/**MMM**M***M**tt!MU*t**M***t  ***************  **********************/ 
PROCEDURE  ASK  !  /*  A  PROCEDURE  WHICH  GIVES  THE  OPTION  TO  ABORT  */ 

<*«C  ■ HCALL  EXIT*S  *) 

VAR  LETTERSCHARI 
BEGIN 

WRITELN( ' DO  YOU  WISH  TO  ABORT?  (Y/N)')! 

RE ADLN (LETTER >  > 

IF  LETTER  *  ' Y '  THEN 
BEGIN 
URITELNS 

WRI TELN ( '  ***  PROGRAM  ABORTED  ***')! 

TURNOFi 

(**C  EXIT*S  *>  /*  EXIT  THE  PROGRAM  */ 

END  ! 

END>  /*  PROCEDURE  ASK  */ 

/************************************************ ************************/ 


/************************************************************************/ 


/*  */ 

/*  turnon:  */ 

/*  (TURN  ON)  IS  A  ROUTINE  WHICH  TURNS  ON  THE  HEXAPOD  MOTOR  t  */ 

/*  ELECTRONICS  POUER.  IT  ASSUMES  THAT  ’TESTON*  HAS  BEEN  CALLED  */ 

/*  SUCCESSFULLY.  IT  SHOULD  BE  C^LED  AT  THE  START  OF  EVERY  */ 

/*  REAL  TIME  SECTION  ( WHEN  TK  ‘VXAPOD  IS  ACTIVELY  SERVOING).  */ 

/*  */ 


/MM********************************************************************/ 

PROCEDURE  TURNON! 

(**C  C0MSTAT=~0166244  .ADDRESS  OF  COMMAND  8  STATUS  WORDS 

F0WER="0166246  ! ADDRESS  OF  POWER  COMMAND  WORD 

PSTAT ="0166176  ! ADDRESS  OF  POWER  STATUS  WORD 

.MACRO  SEND  Y.Z»?T 

t:  TSTB  COMSTAT  .CHECK  IF  FORWARD  PATH  busy 

BPL  T 

MOV  Y.Z  ! SEND  DATA 

.  ENDM 

*) 


BEGIN 


<**C 


♦4.PSTAT 

KIL 


.TEST  KILL  CIRCUIT  STATUS 
SBRANCH  IF  INACTIVE 


URITELNS 

URITELN( ' THE  KILL  CIRCUIT  IS  ACTIVE.  PLEASE  DEACTI VATE . ' > S 


SMC 

uaitk:  bit 

BNE 

*) 


*4.PSTAT 

UAITK 


8 TEST  KILL  CIRCUIT 
SLOOP  UNTIL  DEACTIVATED 


URITELN<  '  THANK  YOU'). 


*3 >  POWER 


.TURN  ON  ELECTRONICS  l  MOTOR  POUEt. 


PUAITS 

PUAITS 


CMC 

MOV 

0*PSTAT.R1 

SCHECK  FOR  MP  t  EP  ON 

BIC 

*"0177774. R1 

SMASK  OFF  MP  l  EP  STATUS 

BITS 

CMP 

*3.  R1 

SCHECK  IF  BOTH  BITS  ON 

BEQ 

DONE 

S  BRANCH  IF  CORRECT 

*) 

URITELNS 

URITELNS 

'  POUER 

CONTROLLER  MALFUNCTION')) 

ASKS 

URITELNS 

URITE( 'PLEASE  SWITCH  TO  MANUAL 

MODE. ' > i 

URITELNS 

'  THEN 

TURN  ON  YELLOW 

l  GREEN  LIGHTS' >  S 

SMC 

uaitp: 

MOV 

0*PST AT  »R1 

SCHECK  FOR  MP  t  EP  ON 

BIC 

*"0177774.  R1 

SMASK  OFF  MP  t  EP  STATUS 

BITS 

CMP 

»3.R1 

SCHECK  IF  BOTH  BITS  ON 

*> 

BNE 

UAITP 

SLOOP  UNTIL  POUER  ON 

URITELNS 

URITELNS 

'  THANK 

YOU'  )  S 

ENDS  /*  TURNON  */ 


/kmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmm*/ 


I 


Td-A125  466 
UNCLASSIFIED 


AN  EXPERIMENTAL  STUDV  OF  AN  ULTRA-NOBILE  VEHICLE  FOR'  2/2 
OFF-ROAD  TRANSPORTATIONS)  OHIO  STATE  UNIV  RESEARCH 
FOUNDATION  COLUMBUS  R  B  MCGHEE  ET  AL.  FEB  82 
NDA903-82-K-885B  F/G  6/4  NL 


MICROCOPY  RESOLUTION  TEST  CHART 

NATIONAL  BUREAU  OF  STANDARDS  1%J  A 


/t******t****t*tts**t**t*****stt*t*ttt***tttt*ttttttt***tt*ts*******$****/ 


/*  */ 
/*  teston:  */ 

/*  (TEST  •  TURN  ON)  IS  AN  INTERACTIVE  PROCEDURE  WHICH  SHOULD  BE  */ 
/*  CALLED  AT  THE  START  OF  EVERY  REAL-TIME  HEXAPOD  CONTROL  PROGRAM.  */ 
/*  IT  AUTOMATICALLY  TESTS  DATA  LINK  OPERATION  AND  THEN  TURNS  ON  */ 
/*  THE  POWER  SYSTEMS  IN  THE  PROPER  SEQUENCE.  ANY  ABNORMAL  */ 
/»  CONDITIONS  ARE  REPORTED  TO  THE  OPERATOR.  */ 
/%  */ 


/***************************«********************************************/ 

PROCEDURE  TESTON) 

VAR  I* 

N. 

DATA. 

LOC! 

INTEGER) 

(**C  AD  BASE  =  '’01 66000 

DABASE* "01 66200 
DABUF” "01 66236 
C0MSTAT”"0166244 

PST AT -"01 661 7 6 
POWER” "01 66246 

.MACRO  SEND  Y.Z»?T 

t:  tstb  comstat 

BPL  T 

MOV  Y.Z 

.  ENDM 

*) 

BEGIN 

WRITELN5 

WRITE( 'PLEASE  TURN  ON  DATA  LINK  POWER  (RED  LIGHT)*')) 

WRITELN( '  THEN  ENTER  A  CARRIAGE  RETURN.')) 

READLN ) 


)  BASE  ADDRESS  OR  A/D'S 
•BASE  ADDRESS  OF  D/A'S 
) D/A  CHANNEL  USED  FOR  FORWARD  TESTS 
) ADDRESS  OF  FORWARD  STATUS  WORD 
)  AND  MODE  COMMAND  WORD 
) ADDRESS  OF  POWER  STATUS  WORD 
) ADDRESS  OF  POWER  COMMAND  WORD 


•CHECK  IF  FORWARD  PATH  BUSY 
)SEND  DATA 


(*******  FEEDBACK  TEST  ROUTINE  *******> 


WRITELN( '  PERFORMING  DATA  LINK  FEEDBACK  TEST')) 


use 

*) 


SEND  *0> POWER 

SEND  SI. COMSTAT 


) TURN  OFF  POWER  SYSTEMS 
•COMMAND  TEST  MODE  *1 
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PWAIT  > 

PWAIT ) 

n:«o> 

FOR  l:=l  TO  (MAXINT  DIV  64)  DO 


BEGIN 

<m 

MOV 

#'*077.  R1 

) INITIALIZE  ADDRESS  COUNTER 

loop: 

ASL 

R1 

)CONVERT  TO  WORD  ADDRESS 

MOV 

ADBASE ( R1  )  >  R2 

) FETCH  DATA 

ASR 

R1 

)RESTORE  COUNTER 

MOV 

R1.R3 

)GENERATE  EXPECTED  DATA  IN  R3 

SWAB 

R3 

)  BY  DUPLICATING  BITS  0-3 

ASR 

R3 

)  IN  BITS  6-9 

ASR 

R3 

ADD 

R1.R3 

BIC 

*~OI76000.R3 

)DATA  NOW  GENERATED 

CMP 

R2.R3 

) CHECK  IF  DATA  IS  CORRECT 

BEQ 

CONT 

*) 

MOV 

Rl.LOC(SP) 

) SAVE  ADDRESS  OF  BAD  DATA 

N1“N+1 ) 
IF  N<20 

THEN 

BEGIN 

WRITE ( '  ERROR  DETECTED  ON  LOOP'. It '>  ADDRESS LOC ) ) 

WRITELN) 

END) 

<MC 

cont: 

DEC 

R1 

‘.DECREMENT  ADDRESS  COUNTER 

BPL 

LOOP 

.LOOP  UNTIL  R1<0 

*) 

END)  /*  FOR  I  */ 

IF  N  <>  0  THEN 
BEGIN 
URITELN) 

WRITELNIN. '  TRANSMISSION  ERRORS  DETECTED  IN'  .  HAXINT  r '  TESTS.')) 
ASK) 

END) 


(«*****«  FEEDFORWARD  TEST  ROUTINE  «****»*) 

WRITELN) 

URITELN< '  PERFORMING  DATA  LINK  FEEDFORWARD  TEST')) 

CMC  SEND  ♦2»C0MSTAT  ) COMMAND  TEST  MODE  *2 

*) 
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n  :  =0 » 

PUAIT ( 

FOR  DATAt«0  TO  127  DO 
BEGIN 

(*«C  SEND  DATA(SP) .DABUF  i SEND  DATA 

*> 


PUAIT) 


(**C 

MOV 

B6ADBASE.R1 

) READ  BACK 

DATA 

CMPB 

Rl.DATA(SP) 

)COMPARE  DATA  SENT  t  RECIEVED 

BEQ 

GOOD 

(BRANCH  IF 

DATA  GOOD 

INC 

N  ( SP ) 

) INCREMENT 

ERROR  COUNTER 

good: 

COM 

DATA  <  SP  > 

(FLIP  BITS 

FOR  NEXT  OUTPUT 

SEND 

DAT  A ( SP ) »  DABUF 

(SEND  DATA 

*) 


PUAIT  5 


<**C 

MOV 

etADBASE .R1 

(READ  BACK  DATA 

CMPB 

R1 « DATA ( SP ) 

(COMPARE  DATA  SENT  l  RECIEVED 

BEQ 

G00D2 

(BRANCH  IF  DATA  GOOD 

INC 

N<  SP ) 

(INCREMENT  ERROR  COUNTER 

G00D2 

*) 

:  COM 

DATA(SP) 

(RESTORE  DATA 

END) 

/*  FOR 

DATA  */ 

IF  N  <>  0  THEN 
BEGIN 
URITELN) 

UR ITELN ( N . '  TRANSMISSION  ERRORS  DETECTED  IN  256  TESTS.')) 
ASK) 

END) 


<**««***  POUER  CONTROLLER  MODE  TEST  *****»«) 

<**C  SEND  *0. CONSTAT  .COMMAND  NORMAL  MODE 

*) 

PUAIT) 

PUAIT) 

<**C  TSTB  PSTAT  ) TEST  IF  POUER  CONTROLLER  IN  COMPUTER  MODE 
BMI  COMP  .BRANCH  IF  IT  IS 

*) 

URITELN) 

URITELN< 'PLEASE  PLACE  POUER  CONTROLLER  IN  COMPUTER  MODE.')) 


/*«»*$**  FILES  KYCK34 . MAC  ******/ 


******tt*t*«*t*t***tt*t*****ttt*****t**t**t*t***«t*t****t*t**t* ****** 

programmers:  Charles  klein.  dennis  pugh 

DATE*.  20-APR-82 

global  variables:  none 

purpose:  assembler  level  keyboard  input  for  special-purpose 
use  with  high  level  languages. 

subroutines  contained: 

keyint:  initializes  terminal  characteristics  and  places  an 

INPUT  REQUEST  ON  THE  SYSTEM  QUEUE.  KEYINT  SHOULD  BE 
CALLED  ONLY  ONCE.  NO  ARGUMENTS  ARE  REQUIRED. 

KEYCK:  RETURNS  THE  CHARACTER  TYPED  ON  THE  KEYBOARD  IN  ITS 

ONE  ARGUMENT  (VARIABLE  TYPE  CHAR).  IF  NO  CHARACTER  HAS 
BEEN  INPUT  SINCE  THE  LAST  CALL.  A  NULL  CHARACTER  IS 
RETURNED  (ASCII  0).  SAMPLE  CALL:  KEYCK ( CHARACTER ) f 

kyuait:  is  to  be  called  when  tt  is  desired  to  suspend  program 

EXECUTION  UNTIL  A  CHARACTER  IS  INPUT.  THUS  FREEING  THE 
SYSTEM  FOR  OTHER  TASKS.  EXECUTION  IS  RESUMED  UHEN  AN 
INPUT  IS  RECIEVED.  KEYCK  CAN  THEN  BE  USED  TO  FETCH  THE 
CHARACTER.  NO  ARGUMENTS  ARE  REQUIRED. 

IOKILL:  IS  USED  TO  CANCEL  THE  INPUT  REQUEST  WHEN  A  HIGH-LEVEL 
READ  (READLN)  IS  TO  BE  PERFORMED.  NO  ARGUMENTS  ARE 
REQUIRED. 


inoue: 


IS  CALLED  TO  REASSERT  THE  INPUT  REQUEST  AFTER  AN  IOKILL 
AND  HIGH-LEVEL  READ.  NO  ARGUMENTS  ARE  REQUIRED. 


***  ALL  SUBROUTINES  ARE  IN  FORTRAN  FORMAT.  *** 
********************************************************************* 


.LIST  TTM 

. MCALL  QIO$S.UTSE«S.ALUN*S 

LOCAL  SYMBOL  DEFINITION 
LUN2  »  2 

EFNl  »  11 

EFN2  a  12 


(  LOCAL 
( 

msg: 

tchar: 
iost  : 

i 

( 

keyint: : 


DATA  BLOCKS: 


.PSECT 

•  BLKB 
.EVEN 
.BYTE 

•  BLKU 

.PSECT 


DATA. Dr RM 
1 

TC.FDX.l 

2 


( CHARACTER  BUFFER 

(TERMINAL  CHARACTERISTICS 
(I/O  STATUS  RETURN  LOCATION 


ALUNtS 

♦  LUN2 . ♦*  T I 

(ASSIGN  LUN  TO  TERMINAL 

QlOtS 

#10. ATT  >#LUN2 

(ATTACH  TERMINAL 

QIO*S 

QlOtS 

#SF . SMC i #LUN2 • * • * » <#TCHAR » #2> 
#I0.RNEr#LUN2.#EFN2f.#I0ST.r<#MSGf#l> 

(SET  TO  FULL  DUPLEX 
(ASSERT  INPUT  REQUEST 

BCS 

ERROR 

(SIGNAL  IF  ERROR 

; 

RTS 

PC 

( 

keyck:  : 

TST 

<R5)  + 

(POINT  RS  TO  ARGUMENT 

CLRB 

BO(RS) 

(PUT  NULL  IN  ARGUMENT 

CMPB 

•IS. SUC. IOST 

(TEST  FOR  SUCCESSFUL  READ 

BNE 

RETURN 

(RETURN  IF  NOT 

HOVB 

MSG.80<RS) 

(PUT  CHAR.  IN  ARGUMENT 

inque:  : 

QlOtS 

# 10 . RNE >  #LUN2 * #EFN2 .  .  #IOST . . <#MSG » #1> 

(REASSERT  INPUT  REQUEST 

BCS 

ERROR 

(SIGNAL  IF  ERROR 

return: 

RTS 

PC 

error: 

MOV  *2. 
IOT 

RO 

(SET  ERROR  FLAG 

( 

( 

kyuait: : 


( 

( 

iokill: 


UTSEtS 

RTS 


OIO*S 

MTSE«S 

RTS 


♦EFN2 

»<■ 


(WAIT  FOR  KEYBOARD  INPUT  WITH  LITTLE  OVERHEAD 


IO.  KIL  >  *L.UN2  » #EFN1 

#EFN1 

PC 


(CANCEL  I/O  REQUESTS 
(WAIT  FOR  COMPLETION 


.END 
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I  FILE;  CMPL34.CMD 

» 

>  function;  indirect  cohhand  file  to 

$  COMPILE  1  ASSEMBLE  ALL  FILES 

>  OF  ROBOT  3.4 
) 

PAS  R0BT34*GBLF34  r R0BT34 

MAC  R0BT34=R0BT34 

PAS  PLAN34*GBLF34 » PLAN34 

MAC  PLAN34*PLAN34 

PAS  FQOT34*GBLF34*FOOT34 

MAC  F00T34=F00T34 

PAS  SERV34*GBLF34 • SERV34 

MAC  SERV34*SERV34 

PAS  INIT34“GBLF34»INIT34 

MAC  INIT34»INIT34 

PAS  LINE34*GBLF34iLINE34 

MAC  L1NE34^LINE34 

PAS  LIBR34-GBLF34.LIBR34 

MAC  LIBR34-LIBR34 

PAS  DLNK34*GBLF34  »  DLNK34 

MAC  0LNK34=DLNK34 

PAS  P0UR34-P0UR34 

MAC  PQUR34=P0WR34 

MAC  KYCK34-KYCK34 

TKB  BR0BT34 


*  FILEJ  R0BT34.CMD 
t 

I  function:  provides  linkage  information 

I  FOR  THE  TASK  BUILDER 

> 

R0BT34/FP»R0BT34/CR/-SP=R0BT34 

PLAN34 

F00T34 

LIBR34 

SERV34 

DLNK34 

INIT34 

LINE34 

KYCK34 

P0UR34 

Cl» 13PASLIB/LB 
/ 

COMMON = I OP AGE ! RU 

•  Ktsct>**h«af> :  5000 
UNITS-7 

// 
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